From 1575d0321dc5d3aebbb8e7cdb0982129748771e1 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Fri, 22 Dec 2017 18:48:26 +0100 Subject: [PATCH] Revert clang-format modifications introduced with column width at 80 in sample code between doxygen \code and \endcode tags) --- demo/wireframe-simulator/servoSimu4Points.cpp | 48 +- .../wireframe-simulator/servoSimuCylinder.cpp | 39 +- demo/wireframe-simulator/servoSimuSphere.cpp | 57 +- example/calibration/calibrateTsai.cpp | 16 +- example/calibration/camera_calibration.cpp | 131 +- .../simulateCircle2DCamVelocity.cpp | 20 +- ...mulateFourPoints2DCartesianCamVelocity.cpp | 32 +- .../simulateFourPoints2DPolarCamVelocity.cpp | 42 +- example/device/display/displayD3D.cpp | 32 +- example/device/display/displayGDI.cpp | 32 +- example/device/display/displayGTK.cpp | 40 +- example/device/display/displayOpenCV.cpp | 41 +- example/device/display/displaySequence.cpp | 52 +- example/device/display/displayX.cpp | 40 +- example/device/display/displayXMulti.cpp | 46 +- example/device/framegrabber/grab1394CMU.cpp | 27 +- example/device/framegrabber/grab1394Two.cpp | 178 +- .../device/framegrabber/grabDirectShow.cpp | 15 +- .../framegrabber/grabDirectShowMulti.cpp | 77 +- example/device/framegrabber/grabDisk.cpp | 53 +- .../device/framegrabber/grabFlyCapture.cpp | 21 +- example/device/framegrabber/grabOpenCV.cpp | 6 +- example/device/framegrabber/grabRealSense.cpp | 116 +- .../device/framegrabber/grabRealSense2.cpp | 106 +- example/device/framegrabber/grabV4l2.cpp | 47 +- .../framegrabber/grabV4l2MultiCpp11Thread.cpp | 81 +- example/device/kinect/kinectAcquisition.cpp | 12 +- example/device/laserscanner/SickLDMRS-Acq.cpp | 10 +- .../device/laserscanner/SickLDMRS-Process.cpp | 76 +- example/device/light/ringLight.cpp | 3 +- .../photometricVisualServoing.cpp | 41 +- example/homography/homographyHLM2DObject.cpp | 12 +- example/homography/homographyHLM3DObject.cpp | 15 +- .../homographyHartleyDLT2DObject.cpp | 6 +- .../homography/homographyRansac2DObject.cpp | 9 +- example/image/imageDiskRW.cpp | 44 +- example/key-point/fernClassifier.cpp | 51 +- example/key-point/keyPointSurf.cpp | 48 +- example/key-point/planarObjectDetector.cpp | 62 +- .../manGeometricFeatures.cpp | 3 +- .../manual/hello-world/CMake/HelloWorld.cpp | 4 +- .../manual/image-manipulation/manDisplay.cpp | 3 +- .../manual/moments/manServoMomentsSimple.cpp | 38 +- example/manual/ogre/HelloWorldOgre.cpp | 9 +- .../manual/ogre/HelloWorldOgreAdvanced.cpp | 13 +- .../simulation/manServo4PointsDisplay.cpp | 6 +- example/manual/simulation/manSimu4Dots.cpp | 10 +- example/manual/simulation/manSimu4Points.cpp | 6 +- example/math/BSpline.cpp | 37 +- example/math/exponentialMap.cpp | 9 +- example/moments/image/servoMomentImage.cpp | 64 +- example/moments/points/servoMomentPoints.cpp | 61 +- .../moments/polygon/servoMomentPolygon.cpp | 56 +- example/ogre-simulator/AROgre.cpp | 108 +- example/ogre-simulator/AROgreBasic.cpp | 56 +- example/parse-argv/parse-argv1.cpp | 15 +- example/parse-argv/parse-argv2.cpp | 33 +- example/pose-estimation/poseVirtualVS.cpp | 59 +- .../servoSimuAfma6FourPoints2DCamVelocity.cpp | 52 +- .../camera/servoSimu3D_cMcd_CamVelocity.cpp | 16 +- ...oSimu3D_cMcd_CamVelocityWithoutVpServo.cpp | 25 +- .../camera/servoSimu3D_cdMc_CamVelocity.cpp | 13 +- ...oSimu3D_cdMc_CamVelocityWithoutVpServo.cpp | 28 +- .../camera/servoSimuCircle2DCamVelocity.cpp | 12 +- .../servoSimuCircle2DCamVelocityDisplay.cpp | 27 +- .../servoSimuCylinder2DCamVelocityDisplay.cpp | 30 +- ...inder2DCamVelocityDisplaySecondaryTask.cpp | 43 +- .../servoSimuFourPoints2DCamVelocity.cpp | 18 +- ...ervoSimuFourPoints2DCamVelocityDisplay.cpp | 45 +- ...imuFourPoints2DPolarCamVelocityDisplay.cpp | 82 +- .../servoSimuLine2DCamVelocityDisplay.cpp | 31 +- .../camera/servoSimuPoint2DCamVelocity1.cpp | 9 +- .../camera/servoSimuPoint2DCamVelocity2.cpp | 27 +- .../camera/servoSimuPoint2DCamVelocity3.cpp | 21 +- .../servoSimuPoint2DhalfCamVelocity1.cpp | 24 +- .../servoSimuPoint2DhalfCamVelocity2.cpp | 18 +- .../servoSimuPoint2DhalfCamVelocity3.cpp | 18 +- .../camera/servoSimuPoint3DCamVelocity.cpp | 16 +- .../camera/servoSimuSphere2DCamVelocity.cpp | 12 +- ...voSimuSphere2DCamVelocitySecondaryTask.cpp | 12 +- ...ervoSimuSquareLine2DCamVelocityDisplay.cpp | 31 +- .../camera/servoSimuThetaUCamVelocity.cpp | 22 +- ...rvoSimuViper850FourPoints2DCamVelocity.cpp | 52 +- example/servo-afma4/moveAfma4.cpp | 3 +- .../servoAfma4Point2DArtVelocity.cpp | 22 +- .../servoAfma4Point2DCamVelocity.cpp | 22 +- .../servoAfma4Point2DCamVelocityKalman.cpp | 43 +- .../servoAfma62DhalfCamVelocity.cpp | 28 +- .../servoAfma6Cylinder2DCamVelocity.cpp | 16 +- ...fma6Cylinder2DCamVelocitySecondaryTask.cpp | 19 +- .../servoAfma6Ellipse2DCamVelocity.cpp | 16 +- .../servoAfma6FourPoints2DArtVelocity.cpp | 29 +- ...rPoints2DCamVelocityInteractionCurrent.cpp | 40 +- ...rPoints2DCamVelocityInteractionDesired.cpp | 35 +- .../servoAfma6Line2DCamVelocity.cpp | 13 +- .../servoAfma6Point2DArtVelocity.cpp | 22 +- .../servoAfma6Point2DCamVelocity.cpp | 9 +- ...servoAfma6Points2DCamVelocityEyeToHand.cpp | 24 +- .../servoAfma6Segment2DCamVelocity.cpp | 6 +- .../servoAfma6SquareLines2DCamVelocity.cpp | 16 +- .../servoAfma6TwoLines2DCamVelocity.cpp | 16 +- example/servo-biclops/moveBiclops.cpp | 30 +- .../servoBiclopsPoint2DArtVelocity.cpp | 40 +- .../servo-franka/franka_echo_robot_state.cpp | 6 +- .../franka_execute_trajectory.cpp | 34 +- .../franka_generate_cartesian_pose_motion.cpp | 29 +- ...nka_generate_cartesian_velocity_motion.cpp | 57 +- .../franka_generate_consecutive_motions.cpp | 34 +- .../franka_generate_joint_position_motion.cpp | 37 +- .../franka_generate_joint_velocity_motion.cpp | 38 +- example/servo-franka/franka_grasp_object.cpp | 17 +- .../franka_joint_impedance_control.cpp | 78 +- .../franka_joint_point_to_point_motion.cpp | 138 +- .../franka_motion_with_control.cpp | 86 +- .../franka_print_joint_positions.cpp | 21 +- example/servo-pioneer/movePioneer.cpp | 24 +- .../servoPioneerPanSegment3D.cpp | 41 +- .../servoPioneerPoint2DDepth.cpp | 20 +- ...servoPioneerPoint2DDepthWithoutVpServo.cpp | 20 +- example/servo-pioneer/sonarPioneerReader.cpp | 47 +- example/servo-ptu46/movePtu46.cpp | 6 +- .../servoPtu46Point2DArtVelocity.cpp | 22 +- ...rPoints2DArtVelocityInteractionCurrent.cpp | 39 +- ...s2DCamVelocityInteractionCurrent-SR300.cpp | 44 +- ...rPoints2DCamVelocityInteractionCurrent.cpp | 39 +- .../servoViper650Point2DCamVelocity.cpp | 18 +- ...rPoints2DArtVelocityInteractionCurrent.cpp | 37 +- ...rPoints2DArtVelocityInteractionDesired.cpp | 32 +- ...rPoints2DCamVelocityInteractionCurrent.cpp | 37 +- .../servoViper850FourPointsKinect.cpp | 40 +- ...oint2DArtVelocity-jointAvoidance-basic.cpp | 17 +- ...0Point2DArtVelocity-jointAvoidance-gpa.cpp | 12 +- ...oint2DArtVelocity-jointAvoidance-large.cpp | 37 +- .../servoViper850Point2DArtVelocity.cpp | 22 +- .../servoViper850Point2DCamVelocity.cpp | 18 +- .../servoViper850Point2DCamVelocityKalman.cpp | 23 +- example/tools/histogram.cpp | 54 +- example/tools/keyboard.cpp | 3 +- example/tools/parallelPort.cpp | 6 +- example/tools/plot3d.cpp | 3 +- example/tracking/mbtEdgeKltMultiTracking.cpp | 77 +- example/tracking/mbtEdgeKltTracking.cpp | 64 +- example/tracking/mbtEdgeMultiTracking.cpp | 76 +- example/tracking/mbtEdgeTracking.cpp | 62 +- example/tracking/mbtGenericTracking.cpp | 125 +- example/tracking/mbtGenericTracking2.cpp | 160 +- example/tracking/mbtGenericTrackingDepth.cpp | 277 +-- example/tracking/mbtKltMultiTracking.cpp | 72 +- example/tracking/mbtKltTracking.cpp | 59 +- example/tracking/templateTracker.cpp | 78 +- example/tracking/trackDot.cpp | 58 +- example/tracking/trackDot2.cpp | 72 +- .../tracking/trackDot2WithAutoDetection.cpp | 91 +- example/tracking/trackKltOpencv.cpp | 52 +- example/tracking/trackMeCircle.cpp | 35 +- example/tracking/trackMeEllipse.cpp | 35 +- example/tracking/trackMeLine.cpp | 38 +- example/tracking/trackMeNurbs.cpp | 35 +- example/video/imageSequenceReader.cpp | 59 +- example/video/videoReader.cpp | 42 +- .../wireframeSimulator.cpp | 21 +- modules/ar/include/visp3/ar/vpAR.h | 9 +- modules/ar/include/visp3/ar/vpAROgre.h | 86 +- modules/ar/include/visp3/ar/vpSimulator.h | 14 +- modules/ar/src/coin-simulator/vpAR.cpp | 52 +- modules/ar/src/coin-simulator/vpSimulator.cpp | 92 +- modules/ar/src/coin-simulator/vpViewer.cpp | 25 +- modules/ar/src/ogre-simulator/vpAROgre.cpp | 280 +-- modules/ar/src/vpSimulatorException.cpp | 15 +- modules/core/include/visp3/core/vpArray2D.h | 79 +- modules/core/include/visp3/core/vpBSpline.h | 31 +- .../include/visp3/core/vpCameraParameters.h | 66 +- modules/core/include/visp3/core/vpCircle.h | 24 +- modules/core/include/visp3/core/vpClient.h | 32 +- modules/core/include/visp3/core/vpColVector.h | 49 +- modules/core/include/visp3/core/vpColor.h | 152 +- modules/core/include/visp3/core/vpConvert.h | 42 +- modules/core/include/visp3/core/vpCylinder.h | 18 +- modules/core/include/visp3/core/vpDebug.h | 52 +- modules/core/include/visp3/core/vpDisplay.h | 481 ++-- .../include/visp3/core/vpDisplayException.h | 5 +- modules/core/include/visp3/core/vpException.h | 7 +- .../include/visp3/core/vpExponentialMap.h | 6 +- .../include/visp3/core/vpFeatureDisplay.h | 50 +- .../include/visp3/core/vpForceTwistMatrix.h | 41 +- .../include/visp3/core/vpForwardProjection.h | 21 +- .../visp3/core/vpFrameGrabberException.h | 12 +- modules/core/include/visp3/core/vpGEMM.h | 185 +- modules/core/include/visp3/core/vpGaussRand.h | 3 +- modules/core/include/visp3/core/vpHistogram.h | 21 +- .../core/include/visp3/core/vpHistogramPeak.h | 3 +- .../include/visp3/core/vpHistogramValey.h | 6 +- .../include/visp3/core/vpHomogeneousMatrix.h | 20 +- modules/core/include/visp3/core/vpImage.h | 298 +-- .../core/include/visp3/core/vpImageConvert.h | 246 +- .../include/visp3/core/vpImageException.h | 6 +- .../core/include/visp3/core/vpImageFilter.h | 215 +- .../include/visp3/core/vpImageMorphology.h | 33 +- .../core/include/visp3/core/vpImagePoint.h | 48 +- .../core/include/visp3/core/vpImageTools.h | 309 +-- .../core/include/visp3/core/vpIoException.h | 5 +- modules/core/include/visp3/core/vpIoTools.h | 81 +- .../core/include/visp3/core/vpKalmanFilter.h | 6 +- modules/core/include/visp3/core/vpLine.h | 15 +- .../core/vpLinearKalmanFilterInstantiation.h | 25 +- modules/core/include/visp3/core/vpList.h | 95 +- modules/core/include/visp3/core/vpMath.h | 134 +- modules/core/include/visp3/core/vpMatrix.h | 205 +- .../include/visp3/core/vpMatrixException.h | 6 +- .../visp3/core/vpMeterPixelConversion.h | 36 +- modules/core/include/visp3/core/vpMoment.h | 3 +- .../core/include/visp3/core/vpMomentAlpha.h | 86 +- .../core/include/visp3/core/vpMomentArea.h | 3 +- .../visp3/core/vpMomentAreaNormalized.h | 23 +- .../core/include/visp3/core/vpMomentBasic.h | 3 +- .../include/visp3/core/vpMomentCInvariant.h | 51 +- .../include/visp3/core/vpMomentCentered.h | 21 +- .../core/include/visp3/core/vpMomentCommon.h | 3 +- .../include/visp3/core/vpMomentDatabase.h | 43 +- .../visp3/core/vpMomentGravityCenter.h | 40 +- .../core/vpMomentGravityCenterNormalized.h | 6 +- .../core/include/visp3/core/vpMomentObject.h | 25 +- modules/core/include/visp3/core/vpMutex.h | 15 +- modules/core/include/visp3/core/vpNetwork.h | 139 +- .../visp3/core/vpPixelMeterConversion.h | 44 +- modules/core/include/visp3/core/vpPlane.h | 15 +- modules/core/include/visp3/core/vpPoint.h | 18 +- modules/core/include/visp3/core/vpPolygon.h | 35 +- modules/core/include/visp3/core/vpPolygon3D.h | 60 +- .../core/include/visp3/core/vpPoseVector.h | 29 +- .../include/visp3/core/vpQuaternionVector.h | 6 +- modules/core/include/visp3/core/vpRGBa.h | 12 +- modules/core/include/visp3/core/vpRansac.h | 26 +- modules/core/include/visp3/core/vpRect.h | 6 +- modules/core/include/visp3/core/vpRequest.h | 19 +- modules/core/include/visp3/core/vpRobust.h | 17 +- .../include/visp3/core/vpRotationMatrix.h | 9 +- .../include/visp3/core/vpRotationVector.h | 14 +- modules/core/include/visp3/core/vpRowVector.h | 33 +- modules/core/include/visp3/core/vpScale.h | 4 +- modules/core/include/visp3/core/vpServer.h | 21 +- modules/core/include/visp3/core/vpSphere.h | 12 +- .../core/include/visp3/core/vpSubColVector.h | 6 +- modules/core/include/visp3/core/vpSubMatrix.h | 8 +- .../core/include/visp3/core/vpSubRowVector.h | 6 +- modules/core/include/visp3/core/vpThread.h | 22 +- modules/core/include/visp3/core/vpTime.h | 3 +- .../include/visp3/core/vpTrackingException.h | 6 +- .../include/visp3/core/vpTranslationVector.h | 17 +- modules/core/include/visp3/core/vpTriangle.h | 12 +- modules/core/include/visp3/core/vpUDPClient.h | 30 +- modules/core/include/visp3/core/vpUDPServer.h | 16 +- modules/core/include/visp3/core/vpUniRand.h | 9 +- .../visp3/core/vpVelocityTwistMatrix.h | 31 +- modules/core/include/visp3/core/vpXmlParser.h | 45 +- .../include/visp3/core/vpXmlParserCamera.h | 86 +- .../visp3/core/vpXmlParserHomogeneousMatrix.h | 37 +- .../core/src/camera/vpCameraParameters.cpp | 150 +- .../src/camera/vpMeterPixelConversion.cpp | 22 +- .../src/camera/vpPixelMeterConversion.cpp | 32 +- modules/core/src/camera/vpXmlParserCamera.cpp | 244 +- modules/core/src/display/vpColor.cpp | 43 +- modules/core/src/display/vpDisplay.cpp | 44 +- modules/core/src/display/vpDisplay_impl.h | 240 +- modules/core/src/display/vpDisplay_rgba.cpp | 263 +-- modules/core/src/display/vpDisplay_uchar.cpp | 266 +-- modules/core/src/display/vpFeatureDisplay.cpp | 66 +- modules/core/src/image/vpImageConvert.cpp | 939 +++----- modules/core/src/image/vpImageFilter.cpp | 216 +- modules/core/src/image/vpImageMorphology.cpp | 123 +- modules/core/src/image/vpImagePoint.cpp | 124 +- modules/core/src/image/vpImageTools.cpp | 132 +- modules/core/src/image/vpRGBa.cpp | 13 +- .../core/src/math/kalman/vpKalmanFilter.cpp | 24 +- .../vpLinearKalmanFilterInstantiation.cpp | 79 +- modules/core/src/math/matrix/vpColVector.cpp | 172 +- modules/core/src/math/matrix/vpMatrix.cpp | 477 ++-- .../src/math/matrix/vpMatrix_cholesky.cpp | 23 +- .../src/math/matrix/vpMatrix_covariance.cpp | 52 +- modules/core/src/math/matrix/vpMatrix_lu.cpp | 104 +- modules/core/src/math/matrix/vpMatrix_mul.cpp | 29 +- modules/core/src/math/matrix/vpMatrix_qr.cpp | 69 +- modules/core/src/math/matrix/vpMatrix_svd.cpp | 40 +- modules/core/src/math/matrix/vpRowVector.cpp | 120 +- .../core/src/math/matrix/vpSubColVector.cpp | 25 +- modules/core/src/math/matrix/vpSubMatrix.cpp | 36 +- .../core/src/math/matrix/vpSubRowVector.cpp | 36 +- modules/core/src/math/misc/vpHinkley.cpp | 23 +- modules/core/src/math/misc/vpMath.cpp | 18 +- modules/core/src/math/robust/vpRobust.cpp | 81 +- modules/core/src/math/robust/vpScale.cpp | 30 +- modules/core/src/math/spline/vpBSpline.cpp | 49 +- .../math/transformation/vpExponentialMap.cpp | 45 +- .../transformation/vpForceTwistMatrix.cpp | 56 +- .../transformation/vpHomogeneousMatrix.cpp | 105 +- .../src/math/transformation/vpPoseVector.cpp | 44 +- .../transformation/vpQuaternionVector.cpp | 86 +- .../math/transformation/vpRotationMatrix.cpp | 83 +- .../src/math/transformation/vpRxyzVector.cpp | 36 +- .../src/math/transformation/vpRzyxVector.cpp | 33 +- .../src/math/transformation/vpRzyzVector.cpp | 36 +- .../math/transformation/vpThetaUVector.cpp | 89 +- .../transformation/vpTranslationVector.cpp | 77 +- .../transformation/vpVelocityTwistMatrix.cpp | 78 +- .../vpXmlParserHomogeneousMatrix.cpp | 90 +- modules/core/src/tools/convert/vpConvert.cpp | 105 +- .../src/tools/cpu-features/x86/cpu_x86.cpp | 3 +- .../core/src/tools/exceptions/vpException.cpp | 29 +- modules/core/src/tools/file/vpIoTools.cpp | 256 +-- modules/core/src/tools/geometry/vpPlane.cpp | 26 +- modules/core/src/tools/geometry/vpPolygon.cpp | 112 +- .../core/src/tools/geometry/vpPolygon3D.cpp | 169 +- modules/core/src/tools/geometry/vpRect.cpp | 42 +- .../core/src/tools/geometry/vpTriangle.cpp | 24 +- .../core/src/tools/histogram/vpHistogram.cpp | 91 +- .../src/tools/histogram/vpHistogramPeak.cpp | 19 +- .../src/tools/histogram/vpHistogramValey.cpp | 3 +- modules/core/src/tools/network/vpClient.cpp | 27 +- modules/core/src/tools/network/vpNetwork.cpp | 102 +- modules/core/src/tools/network/vpRequest.cpp | 5 +- modules/core/src/tools/network/vpServer.cpp | 99 +- .../core/src/tools/network/vpUDPClient.cpp | 41 +- .../core/src/tools/network/vpUDPServer.cpp | 82 +- modules/core/src/tools/time/vpTime.cpp | 148 +- modules/core/src/tools/xml/vpXmlParser.cpp | 61 +- .../tracking/forward-projection/vpCircle.cpp | 47 +- .../forward-projection/vpCylinder.cpp | 49 +- .../tracking/forward-projection/vpLine.cpp | 42 +- .../tracking/forward-projection/vpPoint.cpp | 56 +- .../tracking/forward-projection/vpSphere.cpp | 26 +- .../core/src/tracking/moments/vpMoment.cpp | 23 +- .../src/tracking/moments/vpMomentAlpha.cpp | 45 +- .../src/tracking/moments/vpMomentArea.cpp | 12 +- .../moments/vpMomentAreaNormalized.cpp | 22 +- .../src/tracking/moments/vpMomentBasic.cpp | 10 +- .../tracking/moments/vpMomentCInvariant.cpp | 74 +- .../src/tracking/moments/vpMomentCentered.cpp | 24 +- .../src/tracking/moments/vpMomentCommon.cpp | 49 +- .../src/tracking/moments/vpMomentDatabase.cpp | 15 +- .../moments/vpMomentGravityCenter.cpp | 16 +- .../vpMomentGravityCenterNormalized.cpp | 42 +- .../src/tracking/moments/vpMomentObject.cpp | 156 +- modules/core/src/tracking/vpTracker.cpp | 5 +- .../camera/testCameraParametersConversion.cpp | 3 +- modules/core/test/image/testConversion.cpp | 344 +-- modules/core/test/image/testCrop.cpp | 24 +- modules/core/test/image/testCropAdvanced.cpp | 123 +- modules/core/test/image/testImageAddSub.cpp | 172 +- modules/core/test/image/testImageBinarise.cpp | 46 +- .../core/test/image/testImageComparison.cpp | 83 +- modules/core/test/image/testImageFilter.cpp | 138 +- .../core/test/image/testImageMorphology.cpp | 488 ++-- modules/core/test/image/testImageResize.cpp | 108 +- modules/core/test/image/testIoPGM.cpp | 36 +- modules/core/test/image/testIoPPM.cpp | 45 +- .../core/test/image/testPerformanceLUT.cpp | 91 +- modules/core/test/image/testReadImage.cpp | 9 +- .../core/test/image/testUndistortImage.cpp | 36 +- modules/core/test/math/testArray2D.cpp | 16 +- modules/core/test/math/testColVector.cpp | 42 +- .../core/test/math/testKalmanAcceleration.cpp | 6 +- modules/core/test/math/testKalmanVelocity.cpp | 6 +- modules/core/test/math/testMath.cpp | 199 +- modules/core/test/math/testMatrix.cpp | 107 +- .../core/test/math/testMatrixDeterminant.cpp | 74 +- modules/core/test/math/testMatrixInverse.cpp | 164 +- .../test/math/testMatrixPseudoInverse.cpp | 143 +- modules/core/test/math/testPoseVector.cpp | 9 +- modules/core/test/math/testQuaternion.cpp | 72 +- modules/core/test/math/testRobust.cpp | 3 +- modules/core/test/math/testRotation.cpp | 81 +- modules/core/test/math/testRowVector.cpp | 9 +- modules/core/test/math/testSvd.cpp | 55 +- .../core/test/math/testTranslationVector.cpp | 9 +- modules/core/test/network/testClient.cpp | 6 +- modules/core/test/network/testServer.cpp | 6 +- modules/core/test/network/testUDPClient.cpp | 15 +- modules/core/test/network/testUDPServer.cpp | 24 +- .../core/test/tools/convert/testConvert.cpp | 68 +- .../tools/cpu-features/testCPUFeatures.cpp | 15 +- .../core/test/tools/geometry/testPolygon.cpp | 61 +- modules/core/test/tools/geometry/testRect.cpp | 6 +- .../test/tools/histogram/testHistogram.cpp | 70 +- modules/core/test/tools/io/testIoTools.cpp | 398 ++-- .../core/test/tools/threading/testMutex.cpp | 15 +- .../core/test/tools/threading/testThread.cpp | 14 +- .../core/test/tools/threading/testThread2.cpp | 35 +- modules/core/test/tools/time/testTime.cpp | 12 +- modules/core/test/tools/xml/testXmlParser.cpp | 33 +- .../visp3/detection/vpDetectorAprilTag.h | 56 +- .../include/visp3/detection/vpDetectorBase.h | 4 +- .../detection/vpDetectorDataMatrixCode.h | 9 +- .../include/visp3/detection/vpDetectorFace.h | 15 +- .../visp3/detection/vpDetectorQRCode.h | 9 +- .../src/barcode/vpDetectorDataMatrixCode.cpp | 3 +- .../src/barcode/vpDetectorQRCode.cpp | 12 +- modules/detection/src/face/vpDetectorFace.cpp | 18 +- .../detection/src/tag/vpDetectorAprilTag.cpp | 114 +- modules/detection/src/vpDetectorBase.cpp | 16 +- modules/detection/test/testAprilTag.cpp | 87 +- modules/gui/include/visp3/gui/vpD3DRenderer.h | 59 +- modules/gui/include/visp3/gui/vpDisplayD3D.h | 6 +- modules/gui/include/visp3/gui/vpDisplayGDI.h | 13 +- modules/gui/include/visp3/gui/vpDisplayGTK.h | 76 +- .../gui/include/visp3/gui/vpDisplayOpenCV.h | 71 +- .../gui/include/visp3/gui/vpDisplayWin32.h | 69 +- modules/gui/include/visp3/gui/vpDisplayX.h | 73 +- modules/gui/include/visp3/gui/vpGDIRenderer.h | 41 +- modules/gui/include/visp3/gui/vpPlot.h | 72 +- modules/gui/include/visp3/gui/vpPlotCurve.h | 7 +- modules/gui/include/visp3/gui/vpPlotGraph.h | 44 +- .../include/visp3/gui/vpProjectionDisplay.h | 12 +- modules/gui/include/visp3/gui/vpWin32API.h | 16 +- .../gui/include/visp3/gui/vpWin32Renderer.h | 26 +- modules/gui/include/visp3/gui/vpWin32Window.h | 6 +- modules/gui/src/display/vpDisplayGTK.cpp | 406 ++-- modules/gui/src/display/vpDisplayOpenCV.cpp | 706 ++---- modules/gui/src/display/vpDisplayX.cpp | 653 ++---- .../gui/src/display/windows/vpD3DRenderer.cpp | 241 +- .../gui/src/display/windows/vpDisplayD3D.cpp | 16 +- .../gui/src/display/windows/vpDisplayGDI.cpp | 16 +- .../src/display/windows/vpDisplayWin32.cpp | 119 +- .../gui/src/display/windows/vpGDIRenderer.cpp | 167 +- .../gui/src/display/windows/vpWin32API.cpp | 34 +- .../gui/src/display/windows/vpWin32Window.cpp | 17 +- .../vpProjectionDisplay.cpp | 34 +- modules/gui/src/plot/vpPlot.cpp | 169 +- modules/gui/src/plot/vpPlotCurve.cpp | 12 +- modules/gui/src/plot/vpPlotGraph.cpp | 289 +-- modules/gui/test/display/testClick.cpp | 50 +- .../test/display/testDisplayPolygonLines.cpp | 42 +- modules/gui/test/display/testDisplayRoi.cpp | 9 +- .../gui/test/display/testDisplayScaled.cpp | 81 +- modules/gui/test/display/testDisplays.cpp | 17 +- modules/gui/test/display/testMouseEvent.cpp | 78 +- modules/gui/test/display/testVideoDevice.cpp | 50 +- .../gui/test/display/testVideoDeviceDual.cpp | 30 +- .../include/visp3/imgproc/vpContours.h | 62 +- .../imgproc/include/visp3/imgproc/vpImgproc.h | 137 +- modules/imgproc/src/vpCLAHE.cpp | 26 +- modules/imgproc/src/vpConnectedComponents.cpp | 23 +- modules/imgproc/src/vpContours.cpp | 84 +- modules/imgproc/src/vpFloodFill.cpp | 8 +- modules/imgproc/src/vpImgproc.cpp | 102 +- modules/imgproc/src/vpMorph.cpp | 18 +- modules/imgproc/src/vpRetinex.cpp | 67 +- modules/imgproc/src/vpThreshold.cpp | 50 +- modules/imgproc/test/testAutoThreshold.cpp | 68 +- .../imgproc/test/testConnectedComponents.cpp | 107 +- modules/imgproc/test/testContours.cpp | 150 +- modules/imgproc/test/testFloodFill.cpp | 162 +- modules/imgproc/test/testImgproc.cpp | 74 +- modules/io/include/visp3/io/vpDiskGrabber.h | 3 +- modules/io/include/visp3/io/vpImageIo.h | 26 +- modules/io/include/visp3/io/vpKeyboard.h | 3 +- .../visp3/io/vpParallelPortException.h | 6 +- modules/io/include/visp3/io/vpParseArgv.h | 34 +- modules/io/include/visp3/io/vpVideoReader.h | 16 +- modules/io/include/visp3/io/vpVideoWriter.h | 15 +- modules/io/src/image/vpImageIo.cpp | 377 +-- .../io/src/parallel-port/vpParallelPort.cpp | 6 +- modules/io/src/tools/vpKeyboard.cpp | 3 +- modules/io/src/tools/vpParseArgv.cpp | 63 +- modules/io/src/video/vpDiskGrabber.cpp | 65 +- modules/io/src/video/vpVideoReader.cpp | 69 +- modules/io/src/video/vpVideoWriter.cpp | 68 +- modules/robot/include/visp3/robot/vpAfma4.h | 3 +- modules/robot/include/visp3/robot/vpAfma6.h | 32 +- modules/robot/include/visp3/robot/vpBiclops.h | 14 +- .../include/visp3/robot/vpImageSimulator.h | 42 +- modules/robot/include/visp3/robot/vpPioneer.h | 3 +- modules/robot/include/visp3/robot/vpPtu46.h | 3 +- .../robot/include/visp3/robot/vpRingLight.h | 4 +- modules/robot/include/visp3/robot/vpRobot.h | 27 +- .../robot/include/visp3/robot/vpRobotAfma4.h | 28 +- .../robot/include/visp3/robot/vpRobotAfma6.h | 48 +- .../include/visp3/robot/vpRobotBiclops.h | 21 +- .../robot/include/visp3/robot/vpRobotCamera.h | 26 +- .../include/visp3/robot/vpRobotException.h | 5 +- .../include/visp3/robot/vpRobotPioneer.h | 19 +- .../robot/include/visp3/robot/vpRobotPtu46.h | 12 +- .../include/visp3/robot/vpRobotSimulator.h | 5 +- .../include/visp3/robot/vpRobotTemplate.h | 9 +- .../include/visp3/robot/vpRobotViper650.h | 47 +- .../include/visp3/robot/vpRobotViper850.h | 44 +- .../visp3/robot/vpRobotWireFrameSimulator.h | 47 +- .../robot/include/visp3/robot/vpServolens.h | 3 +- .../include/visp3/robot/vpSimulatorAfma6.h | 68 +- .../include/visp3/robot/vpSimulatorCamera.h | 26 +- .../include/visp3/robot/vpSimulatorPioneer.h | 19 +- .../visp3/robot/vpSimulatorPioneerPan.h | 22 +- .../include/visp3/robot/vpSimulatorViper850.h | 54 +- modules/robot/include/visp3/robot/vpViper.h | 16 +- .../robot/include/visp3/robot/vpViper650.h | 26 +- .../robot/include/visp3/robot/vpViper850.h | 26 +- .../robot/include/visp3/robot/vpVirtuose.h | 4 +- .../visp3/robot/vpWireFrameSimulator.h | 205 +- .../src/haptic-device/virtuose/vpVirtuose.cpp | 206 +- .../src/image-simulator/vpImageSimulator.cpp | 221 +- .../robot/src/real-robot/afma4/vpAfma4.cpp | 30 +- .../src/real-robot/afma4/vpRobotAfma4.cpp | 208 +- .../src/real-robot/afma4/vpServolens.cpp | 115 +- .../robot/src/real-robot/afma6/vpAfma6.cpp | 239 +- .../src/real-robot/afma6/vpRobotAfma6.cpp | 201 +- .../src/real-robot/biclops/vpBiclops.cpp | 42 +- .../src/real-robot/biclops/vpRobotBiclops.cpp | 235 +- .../biclops/vpRobotBiclopsController.cpp | 41 +- .../src/real-robot/pioneer/vpRobotPioneer.cpp | 22 +- .../robot/src/real-robot/ptu46/vpPtu46.cpp | 20 +- .../src/real-robot/ptu46/vpRobotPtu46.cpp | 166 +- .../src/real-robot/viper/vpRobotViper650.cpp | 238 +- .../src/real-robot/viper/vpRobotViper850.cpp | 265 +-- .../robot/src/real-robot/viper/vpViper.cpp | 143 +- .../robot/src/real-robot/viper/vpViper650.cpp | 245 +- .../robot/src/real-robot/viper/vpViper850.cpp | 245 +- .../src/robot-simulator/vpRobotCamera.cpp | 33 +- .../vpRobotWireFrameSimulator.cpp | 59 +- .../src/robot-simulator/vpSimulatorAfma6.cpp | 332 +-- .../src/robot-simulator/vpSimulatorCamera.cpp | 35 +- .../robot-simulator/vpSimulatorPioneer.cpp | 44 +- .../robot-simulator/vpSimulatorPioneerPan.cpp | 44 +- .../robot-simulator/vpSimulatorViper850.cpp | 337 +-- modules/robot/src/vpRobot.cpp | 40 +- modules/robot/src/vpRobotTemplate.cpp | 38 +- .../robot/src/wireframe-simulator/vpArit.cpp | 27 +- .../robot/src/wireframe-simulator/vpArit.h | 173 +- .../src/wireframe-simulator/vpAritio.cpp | 11 +- .../robot/src/wireframe-simulator/vpBound.cpp | 9 +- .../src/wireframe-simulator/vpBoundio.cpp | 3 +- .../src/wireframe-simulator/vpClipping.cpp | 26 +- .../src/wireframe-simulator/vpCoreDisplay.cpp | 18 +- .../robot/src/wireframe-simulator/vpLex.cpp | 3 +- modules/robot/src/wireframe-simulator/vpMy.h | 18 +- .../src/wireframe-simulator/vpProjection.cpp | 16 +- .../robot/src/wireframe-simulator/vpScene.cpp | 55 +- .../robot/src/wireframe-simulator/vpView.h | 69 +- .../src/wireframe-simulator/vpVwstack.cpp | 9 +- .../vpWireFrameSimulator.cpp | 291 +-- .../robot/test/servo-afma4/testRobotAfma4.cpp | 3 +- modules/robot/test/servo-afma6/testAfma6.cpp | 22 +- .../robot/test/servo-afma6/testRobotAfma6.cpp | 24 +- .../test/servo-afma6/testRobotAfma6Pose.cpp | 18 +- .../servo-viper/testRobotViper650-frames.cpp | 25 +- .../servo-viper/testRobotViper850-frames.cpp | 25 +- .../test/servo-viper/testRobotViper850.cpp | 9 +- .../servo-viper/testRobotViper850Pose.cpp | 18 +- .../robot/test/servo-viper/testViper650.cpp | 7 +- .../robot/test/servo-viper/testViper850.cpp | 7 +- modules/robot/test/virtuose/testVirtuose.cpp | 3 +- .../robot/test/virtuose/testVirtuoseAfma6.cpp | 15 +- .../test/virtuose/testVirtuoseHapticBox.cpp | 12 +- .../test/virtuose/testVirtuoseJointLimits.cpp | 30 +- .../virtuose/testVirtuosePeriodicFunction.cpp | 6 +- .../include/visp3/sensor/vp1394CMUGrabber.h | 23 +- .../include/visp3/sensor/vp1394TwoGrabber.h | 52 +- .../sensor/include/visp3/sensor/vpComedi.h | 32 +- .../include/visp3/sensor/vpDirectShowDevice.h | 8 +- .../visp3/sensor/vpDirectShowGrabber.h | 3 +- .../visp3/sensor/vpDirectShowGrabberImpl.h | 25 +- .../visp3/sensor/vpDirectShowSampleGrabberI.h | 5 +- .../visp3/sensor/vpFlyCaptureGrabber.h | 36 +- .../visp3/sensor/vpForceTorqueAtiSensor.h | 24 +- .../sensor/include/visp3/sensor/vpKinect.h | 28 +- .../sensor/include/visp3/sensor/vpLaserScan.h | 43 +- .../include/visp3/sensor/vpLaserScanner.h | 3 +- .../include/visp3/sensor/vpOpenCVGrabber.h | 4 +- .../include/visp3/sensor/vpPylonFactory.h | 6 +- .../include/visp3/sensor/vpPylonGrabber.h | 9 +- .../sensor/include/visp3/sensor/vpRealSense.h | 217 +- .../include/visp3/sensor/vpRealSense2.h | 117 +- .../sensor/include/visp3/sensor/vpScanPoint.h | 64 +- .../sensor/include/visp3/sensor/vpSickLDMRS.h | 10 +- .../include/visp3/sensor/vpV4l2Grabber.h | 30 +- modules/sensor/src/force-torque/vpComedi.cpp | 28 +- .../force-torque/vpForceTorqueAtiSensor.cpp | 43 +- .../framegrabber/1394/vp1394CMUGrabber.cpp | 89 +- .../framegrabber/1394/vp1394TwoGrabber.cpp | 725 ++---- .../framegrabber/OpenCV/vpOpenCVGrabber.cpp | 35 +- .../directshow/vpDirectShowDevice.cpp | 3 +- .../directshow/vpDirectShowGrabber.cpp | 44 +- .../directshow/vpDirectShowGrabberImpl.cpp | 313 +-- .../directshow/vpDirectShowSampleGrabberI.cpp | 92 +- .../flycapture/vpFlyCaptureGrabber.cpp | 245 +- .../framegrabber/pylon/vpPylonGrabberGigE.cpp | 88 +- .../framegrabber/pylon/vpPylonGrabberGigE.h | 7 +- .../framegrabber/pylon/vpPylonGrabberUsb.cpp | 94 +- .../framegrabber/pylon/vpPylonGrabberUsb.h | 7 +- .../src/framegrabber/v4l2/vpV4l2Grabber.cpp | 287 +-- .../src/laserscanner/sick/vpSickLDMRS.cpp | 15 +- .../sensor/src/rgb-depth/kinect/vpKinect.cpp | 40 +- .../src/rgb-depth/realsense/vpRealSense.cpp | 383 ++-- .../src/rgb-depth/realsense/vpRealSense2.cpp | 303 +-- .../rgb-depth/realsense/vpRealSense_impl.h | 331 +-- .../sensor/test/force-torque/testComedi.cpp | 17 +- .../test/force-torque/testForceTorqueAti.cpp | 42 +- .../test/framegrabber/test1394TwoGrabber.cpp | 3 +- .../test/framegrabber/testPylonGrabber.cpp | 6 +- .../test/rgb-depth/testRealSense2_SR300.cpp | 197 +- .../test/rgb-depth/testRealSense_R200.cpp | 272 +-- .../test/rgb-depth/testRealSense_SR300.cpp | 196 +- .../tracker/blob/include/visp3/blob/vpDot.h | 188 +- .../tracker/blob/include/visp3/blob/vpDot2.h | 98 +- modules/tracker/blob/src/dots/vpDot.cpp | 132 +- modules/tracker/blob/src/dots/vpDot2.cpp | 451 ++-- .../blob/test/tracking/testTrackDot.cpp | 34 +- .../klt/include/visp3/klt/vpKltOpencv.h | 109 +- modules/tracker/klt/src/vpKltOpencv.cpp | 266 +-- .../include/visp3/mbt/vpMbDepthDenseTracker.h | 63 +- .../visp3/mbt/vpMbDepthNormalTracker.h | 80 +- .../visp3/mbt/vpMbEdgeKltMultiTracker.h | 282 +-- .../include/visp3/mbt/vpMbEdgeKltTracker.h | 126 +- .../include/visp3/mbt/vpMbEdgeMultiTracker.h | 304 +-- .../mbt/include/visp3/mbt/vpMbEdgeTracker.h | 164 +- .../include/visp3/mbt/vpMbGenericTracker.h | 475 ++-- .../mbt/include/visp3/mbt/vpMbHiddenFaces.h | 266 +-- .../include/visp3/mbt/vpMbKltMultiTracker.h | 286 +-- .../mbt/include/visp3/mbt/vpMbKltTracker.h | 135 +- .../mbt/include/visp3/mbt/vpMbScanLine.h | 72 +- .../mbt/include/visp3/mbt/vpMbTracker.h | 258 +-- .../mbt/include/visp3/mbt/vpMbXmlParser.h | 30 +- .../include/visp3/mbt/vpMbtDistanceCircle.h | 47 +- .../include/visp3/mbt/vpMbtDistanceCylinder.h | 49 +- .../visp3/mbt/vpMbtDistanceKltCylinder.h | 39 +- .../visp3/mbt/vpMbtDistanceKltPoints.h | 40 +- .../mbt/include/visp3/mbt/vpMbtDistanceLine.h | 47 +- .../include/visp3/mbt/vpMbtEdgeKltXmlParser.h | 3 +- .../include/visp3/mbt/vpMbtFaceDepthDense.h | 92 +- .../include/visp3/mbt/vpMbtFaceDepthNormal.h | 150 +- .../mbt/include/visp3/mbt/vpMbtMeEllipse.h | 17 +- .../mbt/include/visp3/mbt/vpMbtMeLine.h | 17 +- .../mbt/include/visp3/mbt/vpMbtPolygon.h | 28 +- .../include/visp3/mbt/vpMbtTukeyEstimator.h | 12 +- .../include/visp3/mbt/vpMbtXmlGenericParser.h | 119 +- .../mbt/src/depth/vpMbDepthDenseTracker.cpp | 254 +-- .../mbt/src/depth/vpMbDepthNormalTracker.cpp | 340 +-- .../mbt/src/depth/vpMbtFaceDepthDense.cpp | 297 +-- .../mbt/src/depth/vpMbtFaceDepthNormal.cpp | 529 ++--- .../mbt/src/depth/vpMbtTukeyEstimator.cpp | 76 +- .../mbt/src/edge/vpMbEdgeMultiTracker.cpp | 1171 ++++------ .../tracker/mbt/src/edge/vpMbEdgeTracker.cpp | 651 ++---- .../mbt/src/edge/vpMbtDistanceCircle.cpp | 86 +- .../mbt/src/edge/vpMbtDistanceCylinder.cpp | 98 +- .../mbt/src/edge/vpMbtDistanceLine.cpp | 176 +- .../tracker/mbt/src/edge/vpMbtMeEllipse.cpp | 74 +- modules/tracker/mbt/src/edge/vpMbtMeLine.cpp | 85 +- .../tracker/mbt/src/edge/vpMbtXmlParser.cpp | 153 +- .../src/hybrid/vpMbEdgeKltMultiTracker.cpp | 703 ++---- .../mbt/src/hybrid/vpMbEdgeKltTracker.cpp | 260 +-- .../mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp | 75 +- .../mbt/src/klt/vpMbKltMultiTracker.cpp | 1102 ++++----- .../tracker/mbt/src/klt/vpMbKltTracker.cpp | 391 ++-- .../mbt/src/klt/vpMbtDistanceKltCylinder.cpp | 164 +- .../mbt/src/klt/vpMbtDistanceKltPoints.cpp | 191 +- .../tracker/mbt/src/klt/vpMbtKltXmlParser.cpp | 88 +- .../tracker/mbt/src/vpMbGenericTracker.cpp | 2024 ++++++----------- modules/tracker/mbt/src/vpMbScanLine.cpp | 125 +- modules/tracker/mbt/src/vpMbTracker.cpp | 678 ++---- modules/tracker/mbt/src/vpMbXmlParser.cpp | 84 +- modules/tracker/mbt/src/vpMbtPolygon.cpp | 63 +- .../tracker/mbt/src/vpMbtXmlGenericParser.cpp | 434 ++-- .../tracker/mbt/test/testTukeyEstimator.cpp | 34 +- .../tracker/me/include/visp3/me/vpMeEllipse.h | 34 +- .../tracker/me/include/visp3/me/vpMeLine.h | 44 +- .../tracker/me/include/visp3/me/vpMeNurbs.h | 31 +- .../tracker/me/include/visp3/me/vpMeSite.h | 21 +- .../tracker/me/include/visp3/me/vpMeTracker.h | 8 +- modules/tracker/me/include/visp3/me/vpNurbs.h | 69 +- modules/tracker/me/src/moving-edges/vpMe.cpp | 106 +- .../me/src/moving-edges/vpMeEllipse.cpp | 111 +- .../tracker/me/src/moving-edges/vpMeLine.cpp | 134 +- .../tracker/me/src/moving-edges/vpMeNurbs.cpp | 152 +- .../tracker/me/src/moving-edges/vpMeSite.cpp | 68 +- .../me/src/moving-edges/vpMeTracker.cpp | 55 +- .../tracker/me/src/moving-edges/vpNurbs.cpp | 235 +- modules/tracker/me/test/testNurbs.cpp | 43 +- .../tt/include/visp3/tt/vpTemplateTracker.h | 68 +- .../visp3/tt/vpTemplateTrackerBSpline.h | 3 +- .../visp3/tt/vpTemplateTrackerHeader.h | 9 +- .../vpTemplateTrackerSSDForwardAdditional.h | 15 +- ...vpTemplateTrackerSSDForwardCompositional.h | 6 +- ...vpTemplateTrackerSSDInverseCompositional.h | 6 +- .../visp3/tt/vpTemplateTrackerTriangle.h | 15 +- .../include/visp3/tt/vpTemplateTrackerWarp.h | 42 +- .../visp3/tt/vpTemplateTrackerWarpAffine.h | 24 +- .../tt/vpTemplateTrackerWarpHomography.h | 27 +- .../tt/vpTemplateTrackerWarpHomographySL3.h | 32 +- .../visp3/tt/vpTemplateTrackerWarpRT.h | 24 +- .../visp3/tt/vpTemplateTrackerWarpSRT.h | 24 +- .../tt/vpTemplateTrackerWarpTranslation.h | 27 +- .../vpTemplateTrackerZNCCForwardAdditional.h | 6 +- ...pTemplateTrackerZNCCInverseCompositional.h | 6 +- .../include/visp3/tt/vpTemplateTrackerZone.h | 28 +- .../tt/src/ssd/vpTemplateTrackerSSD.cpp | 15 +- .../tt/src/ssd/vpTemplateTrackerSSDESM.cpp | 21 +- .../vpTemplateTrackerSSDForwardAdditional.cpp | 26 +- ...TemplateTrackerSSDForwardCompositional.cpp | 21 +- ...TemplateTrackerSSDInverseCompositional.cpp | 35 +- .../tt/src/tools/vpTemplateTrackerBSpline.cpp | 6 +- .../src/tools/vpTemplateTrackerTriangle.cpp | 105 +- .../tt/src/tools/vpTemplateTrackerZone.cpp | 82 +- modules/tracker/tt/src/vpTemplateTracker.cpp | 50 +- .../tt/src/warp/vpTemplateTrackerWarp.cpp | 30 +- .../src/warp/vpTemplateTrackerWarpAffine.cpp | 45 +- .../warp/vpTemplateTrackerWarpHomography.cpp | 81 +- .../vpTemplateTrackerWarpHomographySL3.cpp | 66 +- .../tt/src/warp/vpTemplateTrackerWarpRT.cpp | 47 +- .../tt/src/warp/vpTemplateTrackerWarpSRT.cpp | 74 +- .../warp/vpTemplateTrackerWarpTranslation.cpp | 45 +- .../tt/src/zncc/vpTemplateTrackerZNCC.cpp | 15 +- ...vpTemplateTrackerZNCCForwardAdditional.cpp | 32 +- ...emplateTrackerZNCCInverseCompositional.cpp | 61 +- .../include/visp3/tt_mi/vpTemplateTrackerMI.h | 48 +- .../visp3/tt_mi/vpTemplateTrackerMIBSpline.h | 80 +- .../visp3/tt_mi/vpTemplateTrackerMIESM.h | 10 +- .../vpTemplateTrackerMIForwardAdditional.h | 20 +- .../vpTemplateTrackerMIForwardCompositional.h | 6 +- .../vpTemplateTrackerMIInverseCompositional.h | 24 +- .../tt_mi/src/mi/vpTemplateTrackerMI.cpp | 103 +- .../tt_mi/src/mi/vpTemplateTrackerMIESM.cpp | 35 +- .../vpTemplateTrackerMIForwardAdditional.cpp | 58 +- ...pTemplateTrackerMIForwardCompositional.cpp | 42 +- ...pTemplateTrackerMIInverseCompositional.cpp | 107 +- .../src/tools/vpTemplateTrackerMIBSpline.cpp | 141 +- .../include/visp3/vision/vpBasicKeyPoint.h | 95 +- .../include/visp3/vision/vpCalibration.h | 89 +- .../visp3/vision/vpCalibrationException.h | 6 +- .../include/visp3/vision/vpFernClassifier.h | 62 +- .../include/visp3/vision/vpHomography.h | 146 +- .../vision/include/visp3/vision/vpKeyPoint.h | 519 ++--- .../include/visp3/vision/vpKeyPointSurf.h | 57 +- .../visp3/vision/vpLevenbergMarquartd.h | 39 +- .../visp3/vision/vpPlanarObjectDetector.h | 56 +- modules/vision/include/visp3/vision/vpPose.h | 146 +- .../include/visp3/vision/vpPoseException.h | 5 +- .../include/visp3/vision/vpPoseFeatures.h | 306 +-- .../visp3/vision/vpXmlConfigParserKeyPoint.h | 101 +- .../vision/src/calibration/vpCalibration.cpp | 107 +- .../src/calibration/vpCalibrationTools.cpp | 239 +- .../homography-estimation/vpHomography.cpp | 90 +- .../homography-estimation/vpHomographyDLT.cpp | 46 +- .../vpHomographyExtract.cpp | 124 +- .../vpHomographyMalis.cpp | 81 +- .../vpHomographyRansac.cpp | 87 +- .../homography-estimation/vpHomographyVVS.cpp | 41 +- .../vision/src/key-point/vpBasicKeyPoint.cpp | 3 +- .../vision/src/key-point/vpFernClassifier.cpp | 126 +- modules/vision/src/key-point/vpKeyPoint.cpp | 1603 +++++-------- .../vision/src/key-point/vpKeyPointSurf.cpp | 149 +- .../src/key-point/vpPlanarObjectDetector.cpp | 99 +- .../key-point/vpXmlConfigParserKeyPoint.cpp | 128 +- .../pose-estimation/vpLevenbergMarquartd.cpp | 93 +- modules/vision/src/pose-estimation/vpPose.cpp | 102 +- .../src/pose-estimation/vpPoseDementhon.cpp | 77 +- .../src/pose-estimation/vpPoseFeatures.cpp | 90 +- .../src/pose-estimation/vpPoseLagrange.cpp | 43 +- .../vision/src/pose-estimation/vpPoseLowe.cpp | 58 +- .../src/pose-estimation/vpPoseRansac.cpp | 182 +- .../vpPoseVirtualVisualServoing.cpp | 26 +- .../test/homography/testDisplacement.cpp | 15 +- .../vision/test/key-point/testKeyPoint-2.cpp | 78 +- .../vision/test/key-point/testKeyPoint-3.cpp | 26 +- .../vision/test/key-point/testKeyPoint-4.cpp | 72 +- .../vision/test/key-point/testKeyPoint-5.cpp | 44 +- .../vision/test/key-point/testKeyPoint-6.cpp | 112 +- .../vision/test/key-point/testKeyPoint-7.cpp | 287 +-- .../vision/test/key-point/testKeyPoint.cpp | 15 +- .../test/key-point/testSurfKeyPoint.cpp | 39 +- modules/vision/test/pose/testFindMatch.cpp | 17 +- modules/vision/test/pose/testPose.cpp | 43 +- modules/vision/test/pose/testPoseFeatures.cpp | 44 +- modules/vision/test/pose/testPoseRansac.cpp | 14 +- modules/vision/test/pose/testPoseRansac2.cpp | 292 +-- .../visp3/visual_features/vpBasicFeature.h | 18 +- .../visp3/visual_features/vpFeatureBuilder.h | 55 +- .../visp3/visual_features/vpFeatureDepth.h | 22 +- .../visp3/visual_features/vpFeatureEllipse.h | 22 +- .../visual_features/vpFeatureException.h | 6 +- .../visp3/visual_features/vpFeatureLine.h | 42 +- .../visual_features/vpFeatureLuminance.h | 9 +- .../visp3/visual_features/vpFeatureMoment.h | 49 +- .../visual_features/vpFeatureMomentAlpha.h | 9 +- .../visual_features/vpFeatureMomentArea.h | 3 +- .../vpFeatureMomentAreaNormalized.h | 10 +- .../visual_features/vpFeatureMomentBasic.h | 19 +- .../vpFeatureMomentCInvariant.h | 9 +- .../visual_features/vpFeatureMomentCentered.h | 18 +- .../visual_features/vpFeatureMomentCommon.h | 89 +- .../visual_features/vpFeatureMomentDatabase.h | 44 +- .../vpFeatureMomentGravityCenter.h | 55 +- .../vpFeatureMomentGravityCenterNormalized.h | 37 +- .../visp3/visual_features/vpFeaturePoint.h | 15 +- .../visp3/visual_features/vpFeaturePoint3D.h | 30 +- .../visual_features/vpFeaturePointPolar.h | 17 +- .../visp3/visual_features/vpFeatureSegment.h | 24 +- .../visp3/visual_features/vpFeatureThetaU.h | 31 +- .../visual_features/vpFeatureTranslation.h | 25 +- .../visual_features/vpFeatureVanishingPoint.h | 9 +- .../visp3/visual_features/vpGenericFeature.h | 54 +- .../vpFeatureBuilderEllipse.cpp | 13 +- .../feature-builder/vpFeatureBuilderLine.cpp | 10 +- .../feature-builder/vpFeatureBuilderPoint.cpp | 20 +- .../vpFeatureBuilderPointPolar.cpp | 23 +- .../vpFeatureBuilderSegment.cpp | 12 +- .../vpFeatureBuilderVanishingPoint.cpp | 15 +- .../src/visual-feature/vpBasicFeature.cpp | 31 +- .../src/visual-feature/vpFeatureDepth.cpp | 45 +- .../src/visual-feature/vpFeatureEllipse.cpp | 28 +- .../src/visual-feature/vpFeatureLine.cpp | 24 +- .../src/visual-feature/vpFeatureLuminance.cpp | 30 +- .../src/visual-feature/vpFeatureMoment.cpp | 27 +- .../visual-feature/vpFeatureMomentAlpha.cpp | 50 +- .../visual-feature/vpFeatureMomentArea.cpp | 15 +- .../vpFeatureMomentAreaNormalized.cpp | 62 +- .../visual-feature/vpFeatureMomentBasic.cpp | 54 +- .../vpFeatureMomentCInvariant.cpp | 1034 ++++----- .../vpFeatureMomentCentered.cpp | 136 +- .../visual-feature/vpFeatureMomentCommon.cpp | 11 +- .../vpFeatureMomentDatabase.cpp | 15 +- .../vpFeatureMomentGravityCenter.cpp | 26 +- ...vpFeatureMomentGravityCenterNormalized.cpp | 76 +- .../src/visual-feature/vpFeaturePoint.cpp | 31 +- .../src/visual-feature/vpFeaturePoint3D.cpp | 43 +- .../visual-feature/vpFeaturePointPolar.cpp | 35 +- .../src/visual-feature/vpFeatureSegment.cpp | 33 +- .../src/visual-feature/vpFeatureThetaU.cpp | 61 +- .../visual-feature/vpFeatureTranslation.cpp | 78 +- .../vpFeatureVanishingPoint.cpp | 13 +- .../src/visual-feature/vpGenericFeature.cpp | 108 +- .../test/feature/testPoint.cpp | 15 +- modules/vs/include/visp3/vs/vpAdaptiveGain.h | 26 +- modules/vs/include/visp3/vs/vpServo.h | 54 +- modules/vs/include/visp3/vs/vpServoData.h | 13 +- modules/vs/include/visp3/vs/vpServoDisplay.h | 12 +- .../vs/include/visp3/vs/vpServoException.h | 6 +- modules/vs/src/vpAdaptiveGain.cpp | 32 +- modules/vs/src/vpServo.cpp | 324 ++- modules/vs/src/vpServoDisplay.cpp | 19 +- .../test/visual-feature/testFeatureMoment.cpp | 42 +- .../visual-feature/testFeatureSegment.cpp | 29 +- .../bridge/opencv/tutorial-bridge-opencv.cpp | 3 +- .../tutorial-homography-from-points.cpp | 19 +- .../tutorial-pose-from-points-image.cpp | 3 +- .../tutorial-pose-from-points-tracking.cpp | 18 +- .../tutorial-pose-from-qrcode-image.cpp | 30 +- .../tutorial-barcode-detector-live.cpp | 14 +- .../barcode/tutorial-barcode-detector.cpp | 24 +- .../tutorial-face-detector-live-threaded.cpp | 31 +- .../face/tutorial-face-detector-live.cpp | 15 +- .../detection/face/tutorial-face-detector.cpp | 9 +- .../tutorial-matching-keypoint-SIFT.cpp | 18 +- .../tutorial-matching-keypoint-homography.cpp | 33 +- .../matching/tutorial-matching-keypoint.cpp | 13 +- .../tutorial-matching-surf-deprecated.cpp | 7 +- ...al-matching-surf-homography-deprecated.cpp | 30 +- .../object/tutorial-detection-object-mbt.cpp | 41 +- .../object/tutorial-detection-object-mbt2.cpp | 82 +- .../tag/tutorial-apriltag-detector-live.cpp | 65 +- .../tag/tutorial-apriltag-detector.cpp | 76 +- .../grabber/tutorial-grabber-1394-writer.cpp | 10 +- tutorial/grabber/tutorial-grabber-1394.cpp | 8 +- tutorial/grabber/tutorial-grabber-CMU1394.cpp | 3 +- .../grabber/tutorial-grabber-basler-pylon.cpp | 11 +- .../grabber/tutorial-grabber-flycapture.cpp | 9 +- .../tutorial-grabber-opencv-threaded.cpp | 28 +- tutorial/grabber/tutorial-grabber-opencv.cpp | 3 +- .../grabber/tutorial-grabber-realsense.cpp | 7 +- .../tutorial-grabber-v4l2-threaded.cpp | 19 +- tutorial/grabber/tutorial-grabber-v4l2.cpp | 3 +- tutorial/grabber/tutorial-video-reader.cpp | 10 +- tutorial/grabber/tutorial-video-recorder.cpp | 29 +- .../tutorial-image-display-scaled-auto.cpp | 3 +- .../tutorial-image-display-scaled-manu.cpp | 3 +- tutorial/image/tutorial-image-display.cpp | 3 +- .../image/tutorial-image-manipulation.cpp | 10 +- tutorial/image/tutorial-image-viewer.cpp | 6 +- tutorial/image/tutorial-undistort.cpp | 10 +- tutorial/image/tutorial-viewer.cpp | 3 +- .../autothreshold/tutorial-autothreshold.cpp | 25 +- .../tutorial-brightness-adjustment.cpp | 37 +- .../tutorial-connected-components.cpp | 13 +- tutorial/imgproc/contour/tutorial-contour.cpp | 50 +- .../tutorial-contrast-sharpening.cpp | 13 +- .../count-coins/tutorial-count-coins.cpp | 22 +- .../flood-fill/tutorial-flood-fill.cpp | 30 +- .../robot/pioneer/tutorial-pioneer-robot.cpp | 3 +- ...-simu-pioneer-continuous-gain-adaptive.cpp | 9 +- ...-simu-pioneer-continuous-gain-constant.cpp | 9 +- .../pioneer/tutorial-simu-pioneer-pan.cpp | 9 +- .../robot/pioneer/tutorial-simu-pioneer.cpp | 9 +- tutorial/trace/tutorial-trace.cpp | 13 +- .../blob/tutorial-blob-auto-tracker.cpp | 12 +- .../tutorial-blob-tracker-live-firewire.cpp | 10 +- .../blob/tutorial-blob-tracker-live-v4l2.cpp | 9 +- .../tutorial-klt-tracker-live-v4l2.cpp | 20 +- .../tutorial-klt-tracker-with-reinit.cpp | 20 +- .../keypoint/tutorial-klt-tracker.cpp | 25 +- .../edges/tutorial-mb-edge-tracker.cpp | 13 +- .../generic/tutorial-mb-tracker-full.cpp | 15 +- .../generic/tutorial-mb-tracker.cpp | 9 +- .../hybrid/tutorial-mb-hybrid-tracker.cpp | 13 +- .../keypoint/tutorial-mb-klt-tracker.cpp | 13 +- ...utorial-mb-generic-tracker-stereo-mono.cpp | 12 +- .../tutorial-mb-generic-tracker-stereo.cpp | 63 +- .../tutorial-mb-tracker-stereo-mono.cpp | 12 +- .../stereo/tutorial-mb-tracker-stereo.cpp | 111 +- .../tutorial-me-ellipse-tracker.cpp | 10 +- .../moving-edges/tutorial-me-line-tracker.cpp | 6 +- .../tutorial-template-tracker.cpp | 4 +- .../ibvs/tutorial-ibvs-4pts-display.cpp | 20 +- .../tutorial-ibvs-4pts-image-tracking.cpp | 21 +- .../ibvs/tutorial-ibvs-4pts-ogre-tracking.cpp | 52 +- .../ibvs/tutorial-ibvs-4pts-ogre.cpp | 13 +- ...-4pts-plotter-continuous-gain-adaptive.cpp | 3 +- ...torial-ibvs-4pts-plotter-gain-adaptive.cpp | 3 +- .../ibvs/tutorial-ibvs-4pts-plotter.cpp | 3 +- .../tutorial-ibvs-4pts-wireframe-camera.cpp | 20 +- ...torial-ibvs-4pts-wireframe-robot-afma6.cpp | 39 +- ...torial-ibvs-4pts-wireframe-robot-viper.cpp | 43 +- .../visual-servo/ibvs/tutorial-ibvs-4pts.cpp | 3 +- 919 files changed, 23354 insertions(+), 44450 deletions(-) diff --git a/demo/wireframe-simulator/servoSimu4Points.cpp b/demo/wireframe-simulator/servoSimu4Points.cpp index 8441ab41ee..c89754079c 100644 --- a/demo/wireframe-simulator/servoSimu4Points.cpp +++ b/demo/wireframe-simulator/servoSimu4Points.cpp @@ -69,8 +69,7 @@ #ifdef VISP_HAVE_DISPLAY void usage(const char *name, std::string ipath, const char *badparam); -bool getOptions(int argc, const char **argv, std::string &ipath, - bool &display); +bool getOptions(int argc, const char **argv, std::string &ipath, bool &display); /*! @@ -130,8 +129,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - bool &display) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &display) { const char *optarg_; int c; @@ -226,8 +224,7 @@ int main(int argc, const char **argv) // Set initial position of the object in the camera frame vpHomogeneousMatrix cMo(0, 0.1, 2.0, vpMath::rad(35), vpMath::rad(25), 0); // Set desired position of the object in the camera frame - vpHomogeneousMatrix cdMo(0.0, 0.0, 1.0, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cdMo(0.0, 0.0, 1.0, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); // 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 @@ -315,17 +312,15 @@ int main(int argc, const char **argv) vpWireFrameSimulator sim; // Set the scene - sim.initScene(vpWireFrameSimulator::PLATE, - vpWireFrameSimulator::D_STANDARD, list); + sim.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD, list); // Initialize simulator frames - sim.set_fMo(wMo); // Position of the object in the world reference frame + sim.set_fMo(wMo); // Position of the object in the world reference frame sim.setCameraPositionRelObj(cMo); // initial position of the camera sim.setDesiredCameraPosition(cdMo); // desired position of the camera // Set the External camera position - vpHomogeneousMatrix camMf( - vpHomogeneousMatrix(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0)); + vpHomogeneousMatrix camMf(vpHomogeneousMatrix(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0)); sim.setExternalCameraPosition(camMf); // Computes the position of a camera which is fixed in the object frame @@ -353,24 +348,20 @@ int main(int argc, const char **argv) // Display the object frame the world reference frame and the camera // frame - vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo() * cMo.inverse(), - camera, 0.2, vpColor::none); - vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo(), camera, 0.2, - vpColor::none); + vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none); + vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo(), camera, 0.2, vpColor::none); vpDisplay::displayFrame(Iext1, camMf, camera, 0.2, vpColor::none); // Display the world reference frame and the object frame vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none); - vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, - vpColor::none); + vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none); vpDisplay::flush(Iint); vpDisplay::flush(Iext1); vpDisplay::flush(Iext2); std::cout << "Click on a display" << std::endl; - while (!vpDisplay::getClick(Iint, false) && - !vpDisplay::getClick(Iext1, false) && + while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext1, false) && !vpDisplay::getClick(Iext2, false)) { }; } @@ -432,20 +423,14 @@ int main(int argc, const char **argv) // Display the camera frame, the object frame the world reference // frame - vpDisplay::displayFrame(Iext1, - sim.getExternalCameraPosition() * - sim.get_fMo() * cMo.inverse(), - camera, 0.2, vpColor::none); - vpDisplay::displayFrame( - Iext1, sim.getExternalCameraPosition() * sim.get_fMo(), camera, - 0.2, vpColor::none); - vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition(), - camera, 0.2, vpColor::none); + vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2, + vpColor::none); + vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition() * sim.get_fMo(), camera, 0.2, vpColor::none); + vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition(), camera, 0.2, vpColor::none); // Display the world reference frame and the object frame vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none); - vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, - vpColor::none); + vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none); vpDisplay::flush(Iint); vpDisplay::flush(Iext1); @@ -454,8 +439,7 @@ int main(int argc, const char **argv) vpTime::wait(t, sampling_time * 1000); // Wait 40 ms - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } task.print(); diff --git a/demo/wireframe-simulator/servoSimuCylinder.cpp b/demo/wireframe-simulator/servoSimuCylinder.cpp index 457606d188..70a80cb738 100644 --- a/demo/wireframe-simulator/servoSimuCylinder.cpp +++ b/demo/wireframe-simulator/servoSimuCylinder.cpp @@ -197,11 +197,9 @@ int main(int argc, const char **argv) robot.setSamplingTime(sampling_time); // Set initial position of the object in the camera frame - vpHomogeneousMatrix cMo(0, 0.1, 0.3, vpMath::rad(35), vpMath::rad(25), - vpMath::rad(75)); + vpHomogeneousMatrix cMo(0, 0.1, 0.3, vpMath::rad(35), vpMath::rad(25), vpMath::rad(75)); // Set desired position of the object in the camera frame - vpHomogeneousMatrix cdMo(0.0, 0.0, 0.5, vpMath::rad(90), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cdMo(0.0, 0.0, 0.5, vpMath::rad(90), vpMath::rad(0), vpMath::rad(0)); // 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 @@ -245,17 +243,15 @@ int main(int argc, const char **argv) vpWireFrameSimulator sim; // Set the scene - sim.initScene(vpWireFrameSimulator::CYLINDER, - vpWireFrameSimulator::D_STANDARD); + sim.initScene(vpWireFrameSimulator::CYLINDER, vpWireFrameSimulator::D_STANDARD); // Initialize simulator frames - sim.set_fMo(wMo); // Position of the object in the world reference frame + sim.set_fMo(wMo); // Position of the object in the world reference frame sim.setCameraPositionRelObj(cMo); // initial position of the camera sim.setDesiredCameraPosition(cdMo); // desired position of the camera // Set the External camera position - vpHomogeneousMatrix camMf( - vpHomogeneousMatrix(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0)); + vpHomogeneousMatrix camMf(vpHomogeneousMatrix(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0)); sim.setExternalCameraPosition(camMf); // Set the parameters of the cameras (internal and external) @@ -278,18 +274,15 @@ int main(int argc, const char **argv) // Display the object frame the world reference frame and the camera // frame - vpDisplay::displayFrame(Iext, camMf * sim.get_fMo() * cMo.inverse(), - camera, 0.2, vpColor::none); - vpDisplay::displayFrame(Iext, camMf * sim.get_fMo(), camera, 0.2, - vpColor::none); + vpDisplay::displayFrame(Iext, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none); + vpDisplay::displayFrame(Iext, camMf * sim.get_fMo(), camera, 0.2, vpColor::none); vpDisplay::displayFrame(Iext, camMf, camera, 0.2, vpColor::none); vpDisplay::flush(Iint); vpDisplay::flush(Iext); std::cout << "Click on a display" << std::endl; - while (!vpDisplay::getClick(Iint, false) && - !vpDisplay::getClick(Iext, false)) { + while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext, false)) { }; } @@ -388,15 +381,10 @@ int main(int argc, const char **argv) // Display the object frame the world reference frame and the camera // frame - vpDisplay::displayFrame(Iext, - sim.getExternalCameraPosition() * - sim.get_fMo() * cMo.inverse(), - camera, 0.2, vpColor::none); - vpDisplay::displayFrame( - Iext, sim.getExternalCameraPosition() * sim.get_fMo(), camera, - 0.2, vpColor::none); - vpDisplay::displayFrame(Iext, sim.getExternalCameraPosition(), camera, - 0.2, vpColor::none); + vpDisplay::displayFrame(Iext, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2, + vpColor::none); + vpDisplay::displayFrame(Iext, sim.getExternalCameraPosition() * sim.get_fMo(), camera, 0.2, vpColor::none); + vpDisplay::displayFrame(Iext, sim.getExternalCameraPosition(), camera, 0.2, vpColor::none); ; vpDisplay::flush(Iint); @@ -405,8 +393,7 @@ int main(int argc, const char **argv) vpTime::wait(t, sampling_time * 1000); // Wait 40 ms - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } task.print(); diff --git a/demo/wireframe-simulator/servoSimuSphere.cpp b/demo/wireframe-simulator/servoSimuSphere.cpp index 003e0c3149..8b9ef5e0ea 100644 --- a/demo/wireframe-simulator/servoSimuSphere.cpp +++ b/demo/wireframe-simulator/servoSimuSphere.cpp @@ -74,8 +74,7 @@ void usage(const char *name, const char *badparam); bool getOptions(int argc, const char **argv, bool &display); void computeVisualFeatures(const vpSphere &sphere, vpGenericFeature &s); -void computeInteractionMatrix(const vpGenericFeature &s, - const vpSphere &sphere, vpMatrix &L); +void computeInteractionMatrix(const vpGenericFeature &s, const vpSphere &sphere, vpMatrix &L); /*! @@ -173,18 +172,15 @@ void computeVisualFeatures(const vpSphere &sphere, vpGenericFeature &s) double m11 = sphere.get_mu11(); double h2; // if (gx != 0 || gy != 0) - if (std::fabs(gx) > std::numeric_limits::epsilon() || - std::fabs(gy) > std::numeric_limits::epsilon()) + if (std::fabs(gx) > std::numeric_limits::epsilon() || std::fabs(gy) > std::numeric_limits::epsilon()) h2 = (vpMath::sqr(gx) + vpMath::sqr(gy)) / - (4 * m20 * vpMath::sqr(gy) + 4 * m02 * vpMath::sqr(gx) - - 8 * m11 * gx * gy); + (4 * m20 * vpMath::sqr(gy) + 4 * m02 * vpMath::sqr(gx) - 8 * m11 * gx * gy); else h2 = 1 / (4 * m20); double sx = gx * h2 / (sqrt(h2 + 1)); double sy = gy * h2 / (sqrt(h2 + 1)); - double sz = - sqrt(h2 + 1); //(h2-(vpMath::sqr(sx)+vpMath::sqr(sy)-1))/(2*sqrt(h2)); + double sz = sqrt(h2 + 1); //(h2-(vpMath::sqr(sx)+vpMath::sqr(sy)-1))/(2*sqrt(h2)); s.set_s(sx, sy, sz); } @@ -196,8 +192,7 @@ void computeVisualFeatures(const vpSphere &sphere, vpGenericFeature &s) with I3 the 3x3 identity matrix with [s]x the skew matrix related to s */ -void computeInteractionMatrix(const vpGenericFeature &s, - const vpSphere &sphere, vpMatrix &L) +void computeInteractionMatrix(const vpGenericFeature &s, const vpSphere &sphere, vpMatrix &L) { L = 0; L[0][0] = -1 / sphere.getR(); @@ -271,8 +266,7 @@ int main(int argc, const char **argv) // Set initial position of the object in the camera frame vpHomogeneousMatrix cMo(0, 0.1, 2.0, vpMath::rad(35), vpMath::rad(25), 0); // Set desired position of the object in the camera frame - vpHomogeneousMatrix cdMo(0.0, 0.0, 0.8, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cdMo(0.0, 0.0, 0.8, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); // 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 @@ -319,17 +313,15 @@ int main(int argc, const char **argv) vpWireFrameSimulator sim; // Set the scene - sim.initScene(vpWireFrameSimulator::SPHERE, - vpWireFrameSimulator::D_STANDARD); + sim.initScene(vpWireFrameSimulator::SPHERE, vpWireFrameSimulator::D_STANDARD); // Initialize simulator frames - sim.set_fMo(wMo); // Position of the object in the world reference frame + sim.set_fMo(wMo); // Position of the object in the world reference frame sim.setCameraPositionRelObj(cMo); // initial position of the camera sim.setDesiredCameraPosition(cdMo); // desired position of the camera // Set the External camera position - vpHomogeneousMatrix camMf(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), - 0); + vpHomogeneousMatrix camMf(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0); sim.setExternalCameraPosition(camMf); // Computes the position of a camera which is fixed in the object frame @@ -356,24 +348,20 @@ int main(int argc, const char **argv) // Display the object frame the world reference frame and the camera // frame - vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo() * cMo.inverse(), - camera, 0.2, vpColor::none); - vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo(), camera, 0.2, - vpColor::none); + vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none); + vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo(), camera, 0.2, vpColor::none); vpDisplay::displayFrame(Iext1, camMf, camera, 0.2, vpColor::none); // Display the world reference frame and the object frame vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none); - vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, - vpColor::none); + vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none); vpDisplay::flush(Iint); vpDisplay::flush(Iext1); vpDisplay::flush(Iext2); std::cout << "Click on a display" << std::endl; - while (!vpDisplay::getClick(Iint, false) && - !vpDisplay::getClick(Iext1, false) && + while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext1, false) && !vpDisplay::getClick(Iext2, false)) { }; } @@ -425,20 +413,14 @@ int main(int argc, const char **argv) // Display the camera frame, the object frame the world reference // frame - vpDisplay::displayFrame(Iext1, - sim.getExternalCameraPosition() * - sim.get_fMo() * cMo.inverse(), - camera, 0.2, vpColor::none); - vpDisplay::displayFrame( - Iext1, sim.getExternalCameraPosition() * sim.get_fMo(), camera, - 0.2, vpColor::none); - vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition(), - camera, 0.2, vpColor::none); + vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2, + vpColor::none); + vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition() * sim.get_fMo(), camera, 0.2, vpColor::none); + vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition(), camera, 0.2, vpColor::none); // Display the world reference frame and the object frame vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none); - vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, - vpColor::none); + vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none); vpDisplay::flush(Iint); vpDisplay::flush(Iext1); @@ -447,8 +429,7 @@ int main(int argc, const char **argv) vpTime::wait(t, sampling_time * 1000); // Wait 40 ms - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } task.print(); diff --git a/example/calibration/calibrateTsai.cpp b/example/calibration/calibrateTsai.cpp index 20ec436527..d3b521b1a5 100644 --- a/example/calibration/calibrateTsai.cpp +++ b/example/calibration/calibrateTsai.cpp @@ -64,11 +64,9 @@ int main() // transformation. The object // frame is attached to the // calibrartion grid - std::vector wMe( - N); // world to hand (end-effector) transformation + std::vector wMe(N); // world to hand (end-effector) transformation // Output: Result of the calibration - vpHomogeneousMatrix - eMc; // hand (end-effector) to eye (camera) transformation + vpHomogeneousMatrix eMc; // hand (end-effector) to eye (camera) transformation // Initialize an eMc transformation used to produce the simulated input // transformations cMo and wMe @@ -81,8 +79,7 @@ int main() eMc.buildFrom(etc, erc); std::cout << "Simulated hand to eye transformation: eMc " << std::endl; std::cout << eMc << std::endl; - std::cout << "Theta U rotation: " << vpMath::deg(erc[0]) << " " - << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) + std::cout << "Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl; vpColVector v_c(6); // camera velocity used to produce 6 simulated poses @@ -136,13 +133,10 @@ int main() // transformations vpCalibration::calibrationTsai(cMo, wMe, eMc); - std::cout << std::endl - << "Output: hand to eye calibration result: eMc estimated " - << std::endl; + std::cout << std::endl << "Output: hand to eye calibration result: eMc estimated " << std::endl; std::cout << eMc << std::endl; eMc.extract(erc); - std::cout << "Theta U rotation: " << vpMath::deg(erc[0]) << " " - << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) + std::cout << "Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl; return 0; } catch (vpException &e) { diff --git a/example/calibration/camera_calibration.cpp b/example/calibration/camera_calibration.cpp index fb5960cb02..6630e7f8b9 100644 --- a/example/calibration/camera_calibration.cpp +++ b/example/calibration/camera_calibration.cpp @@ -64,8 +64,7 @@ class Settings { public: Settings() - : boardSize(), calibrationPattern(UNDEFINED), squareSize(0.), input(), - tempo(0.), goodInput(false), patternToUse() + : boardSize(), calibrationPattern(UNDEFINED), squareSize(0.), input(), tempo(0.), goodInput(false), patternToUse() { boardSize = cv::Size(0, 0); calibrationPattern = UNDEFINED; @@ -100,8 +99,7 @@ class Settings { goodInput = true; if (boardSize.width <= 0 || boardSize.height <= 0) { - std::cerr << "Invalid Board size: " << boardSize.width << " " - << boardSize.height << std::endl; + std::cerr << "Invalid Board size: " << boardSize.width << " " << boardSize.height << std::endl; goodInput = false; } if (squareSize <= 10e-6) { @@ -118,22 +116,21 @@ class Settings else if (patternToUse.compare("CIRCLES_GRID") == 0) calibrationPattern = CIRCLES_GRID; if (calibrationPattern == UNDEFINED) { - std::cerr << " Inexistent camera calibration mode: " << patternToUse - << std::endl; + std::cerr << " Inexistent camera calibration mode: " << patternToUse << std::endl; goodInput = false; } } public: - cv::Size boardSize; // The size of the board -> Number of items by width and - // height + cv::Size boardSize; // The size of the board -> Number of items by width and + // height Pattern calibrationPattern; // One of the Chessboard, circles, or asymmetric // circle pattern - float squareSize; // The size of a square in your defined unit (point, - // millimeter,etc). - std::string input; // The input image sequence - float tempo; // Tempo in seconds between two images. If > 10 wait a click to - // continue + float squareSize; // The size of a square in your defined unit (point, + // millimeter,etc). + std::string input; // The input image sequence + float tempo; // Tempo in seconds between two images. If > 10 wait a click to + // continue bool goodInput; private: @@ -149,17 +146,13 @@ int main(int argc, const char **argv) Settings s; const std::string inputSettingsFile = argc > 1 ? argv[1] : "default.cfg"; if (!s.read(inputSettingsFile)) { - std::cout << "Could not open the configuration file: \"" - << inputSettingsFile << "\"" << std::endl; - std::cout << std::endl - << "Usage: " << argv[0] << " .cfg" - << std::endl; + std::cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << std::endl; + std::cout << std::endl << "Usage: " << argv[0] << " .cfg" << std::endl; return -1; } if (!s.goodInput) { - std::cout << "Invalid input detected. Application stopping. " - << std::endl; + std::cout << "Invalid input detected. Application stopping. " << std::endl; return -1; } @@ -204,19 +197,16 @@ int main(int argc, const char **argv) // std::cout << "Use chessboard " << std::endl; found = findChessboardCorners(cvI, s.boardSize, pointBuf, #if (VISP_HAVE_OPENCV_VERSION >= 0x030000) - cv::CALIB_CB_ADAPTIVE_THRESH | - cv::CALIB_CB_FAST_CHECK | + cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE); #else - CV_CALIB_CB_ADAPTIVE_THRESH | - CV_CALIB_CB_FAST_CHECK | + CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE); #endif break; case Settings::CIRCLES_GRID: // std::cout << "Use circle grid " << std::endl; - found = findCirclesGrid(cvI, s.boardSize, pointBuf, - cv::CALIB_CB_SYMMETRIC_GRID); + found = findCirclesGrid(cvI, s.boardSize, pointBuf, cv::CALIB_CB_SYMMETRIC_GRID); break; case Settings::UNDEFINED: default: @@ -238,12 +228,9 @@ int main(int argc, const char **argv) // improve the found corners' coordinate accuracy for chessboard cornerSubPix(cvI, pointBuf, cv::Size(11, 11), cv::Size(-1, -1), #if (VISP_HAVE_OPENCV_VERSION >= 0x030000) - cv::TermCriteria(cv::TermCriteria::EPS + - cv::TermCriteria::COUNT, - 30, 0.1)); + cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1)); #else - cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, - 30, 0.1)); + cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); #endif } std::stringstream ss; @@ -260,8 +247,7 @@ int main(int argc, const char **argv) calib.setLambda(0.5); calib.clearPoint(); for (unsigned int i = 0; i < model.size(); i++) { - calib.addPoint(model[i].get_oX(), model[i].get_oY(), - model[i].get_oZ(), data[i]); + calib.addPoint(model[i].get_oX(), model[i].get_oY(), model[i].get_oZ(), data[i]); } vpHomogeneousMatrix cMo; vpCameraParameters cam; @@ -273,23 +259,19 @@ int main(int argc, const char **argv) double v0 = I.getHeight() / 2; cam.initPersProjWithoutDistortion(px, py, u0, v0); - if (calib.computeCalibration(vpCalibration::CALIB_VIRTUAL_VS, cMo, - cam, false) == 0) { + if (calib.computeCalibration(vpCalibration::CALIB_VIRTUAL_VS, cMo, cam, false) == 0) { // std::cout << "camera parameters: " << cam << std::endl; calibrator.push_back(calib); } } if (found) - vpDisplay::displayText(I, 15, 15, "Image processing succeed", - vpColor::green); + vpDisplay::displayText(I, 15, 15, "Image processing succeed", vpColor::green); else - vpDisplay::displayText(I, 15, 15, "Image processing fails", - vpColor::green); + vpDisplay::displayText(I, 15, 15, "Image processing fails", vpColor::green); if (s.tempo > 10.f) { - vpDisplay::displayText(I, 35, 15, "A click to process the next image", - vpColor::green); + vpDisplay::displayText(I, 35, 15, "A click to process the next image", vpColor::green); vpDisplay::flush(I); vpDisplay::getClick(I); } else { @@ -301,15 +283,13 @@ int main(int argc, const char **argv) // Now we consider the multi image calibration // Calibrate by a non linear method based on virtual visual servoing if (calibrator.empty()) { - std::cerr << "Unable to calibrate. Image processing failed !" - << std::endl; + std::cerr << "Unable to calibrate. Image processing failed !" << std::endl; return 0; } std::stringstream ss_additional_info; ss_additional_info << "" << vpTime::getDateTime() << ""; - ss_additional_info << "" << calibrator.size() - << ""; + ss_additional_info << "" << calibrator.size() << ""; ss_additional_info << ""; switch (s.calibrationPattern) { @@ -327,34 +307,27 @@ int main(int argc, const char **argv) break; } ss_additional_info << ""; - ss_additional_info << "" << s.boardSize.width << "x" - << s.boardSize.height << ""; + ss_additional_info << "" << s.boardSize.width << "x" << s.boardSize.height << ""; ss_additional_info << "" << s.squareSize << ""; - std::cout << "\nCalibration without distortion in progress on " - << calibrator.size() << " images..." << std::endl; + std::cout << "\nCalibration without distortion in progress on " << calibrator.size() << " images..." << std::endl; vpCameraParameters cam; double error; - if (vpCalibration::computeCalibrationMulti( - vpCalibration::CALIB_VIRTUAL_VS, calibrator, cam, error, false) == - 0) { + if (vpCalibration::computeCalibrationMulti(vpCalibration::CALIB_VIRTUAL_VS, calibrator, cam, error, false) == 0) { std::cout << cam << std::endl; std::cout << "Global reprojection error: " << error << std::endl; - ss_additional_info << "" - << error << ""; + ss_additional_info << "" << error << ""; #ifdef VISP_HAVE_XML2 vpXmlParserCamera xml; - if (xml.save(cam, outputFileName.c_str(), "Camera", I.getWidth(), - I.getHeight()) == vpXmlParserCamera::SEQUENCE_OK) - std::cout - << "Camera parameters without distortion successfully saved in \"" - << outputFileName << "\"" << std::endl; + if (xml.save(cam, outputFileName.c_str(), "Camera", I.getWidth(), I.getHeight()) == + vpXmlParserCamera::SEQUENCE_OK) + std::cout << "Camera parameters without distortion successfully saved in \"" << outputFileName << "\"" + << std::endl; else { - std::cout - << "Failed to save the camera parameters without distortion in \"" - << outputFileName << "\"" << std::endl; + std::cout << "Failed to save the camera parameters without distortion in \"" << outputFileName << "\"" + << std::endl; std::cout << "A file with the same name exists. Remove it to be able " "to save the parameters..." << std::endl; @@ -363,29 +336,23 @@ int main(int argc, const char **argv) } else std::cout << "Calibration without distortion failed." << std::endl; - std::cout << "\nCalibration with distortion in progress on " - << calibrator.size() << " images..." << std::endl; - if (vpCalibration::computeCalibrationMulti( - vpCalibration::CALIB_VIRTUAL_VS_DIST, calibrator, cam, error, - false) == 0) { + std::cout << "\nCalibration with distortion in progress on " << calibrator.size() << " images..." << std::endl; + if (vpCalibration::computeCalibrationMulti(vpCalibration::CALIB_VIRTUAL_VS_DIST, calibrator, cam, error, false) == + 0) { std::cout << cam << std::endl; std::cout << "Global reprojection error: " << error << std::endl; - ss_additional_info << "" << error - << ""; + ss_additional_info << "" << error << ""; #ifdef VISP_HAVE_XML2 vpXmlParserCamera xml; - if (xml.save(cam, outputFileName.c_str(), "Camera", I.getWidth(), - I.getHeight(), ss_additional_info.str()) == + if (xml.save(cam, outputFileName.c_str(), "Camera", I.getWidth(), I.getHeight(), ss_additional_info.str()) == vpXmlParserCamera::SEQUENCE_OK) - std::cout - << "Camera parameters without distortion successfully saved in \"" - << outputFileName << "\"" << std::endl; + std::cout << "Camera parameters without distortion successfully saved in \"" << outputFileName << "\"" + << std::endl; else { - std::cout - << "Failed to save the camera parameters without distortion in \"" - << outputFileName << "\"" << std::endl; + std::cout << "Failed to save the camera parameters without distortion in \"" << outputFileName << "\"" + << std::endl; std::cout << "A file with the same name exists. Remove it to be able " "to save the parameters..." << std::endl; @@ -393,8 +360,8 @@ int main(int argc, const char **argv) #endif std::cout << std::endl; for (unsigned int i = 0; i < calibrator.size(); i++) - std::cout << "Estimated pose on input data " << i << ": " - << vpPoseVector(calibrator[i].cMo_dist).t() << std::endl; + std::cout << "Estimated pose on input data " << i << ": " << vpPoseVector(calibrator[i].cMo_dist).t() + << std::endl; } else std::cout << "Calibration with distortion failed." << std::endl; @@ -406,9 +373,5 @@ int main(int argc, const char **argv) } } #else -int main() -{ - std::cout << "OpenCV 2.3.0 or higher is requested to run the calibration." - << std::endl; -} +int main() { std::cout << "OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl; } #endif diff --git a/example/coin-simulator/simulateCircle2DCamVelocity.cpp b/example/coin-simulator/simulateCircle2DCamVelocity.cpp index 8c16d31440..b0693aa4e1 100644 --- a/example/coin-simulator/simulateCircle2DCamVelocity.cpp +++ b/example/coin-simulator/simulateCircle2DCamVelocity.cpp @@ -124,8 +124,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - bool &display) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &display) { const char *optarg; int c; @@ -260,8 +259,7 @@ static void *mainLoop(void *_simu) vpColVector v = task.computeControlLaw(); if (iter == 1) { std::cout << "Task rank: " << task.getTaskRank() << std::endl; - std::cout << "send the camera velocity to the controller" - << std::endl; + std::cout << "send the camera velocity to the controller" << std::endl; } robot.setVelocity(vpRobot::CAMERA_FRAME, v); @@ -323,8 +321,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_INPUT_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_INPUT_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -333,13 +330,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } diff --git a/example/coin-simulator/simulateFourPoints2DCartesianCamVelocity.cpp b/example/coin-simulator/simulateFourPoints2DCartesianCamVelocity.cpp index 5acc1c8622..f8dee38e4b 100644 --- a/example/coin-simulator/simulateFourPoints2DCartesianCamVelocity.cpp +++ b/example/coin-simulator/simulateFourPoints2DCartesianCamVelocity.cpp @@ -124,8 +124,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - bool &display) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &display) { const char *optarg; int c; @@ -173,15 +172,12 @@ static void *mainLoop(void *_simu) robot.setSamplingTime(sampling_time); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout << " Eye-in-hand task control, articular velocities are computed" - << std::endl; + std::cout << " Eye-in-hand task control, articular velocities are computed" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo 4 points " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // Sets the initial camera location @@ -223,8 +219,7 @@ static void *mainLoop(void *_simu) // Sets the desired position of the point vpFeaturePoint p[4]; for (int i = 0; i < 4; i++) - vpFeatureBuilder::create( - p[i], point[i]); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure // Sets the desired position of the point vpFeaturePoint pd[4]; @@ -260,8 +255,7 @@ static void *mainLoop(void *_simu) std::cout << "Display task information" << std::endl; task.print(); - vpTime::wait( - 1000); // Sleep 1s to ensure that all the thread are initialized + vpTime::wait(1000); // Sleep 1s to ensure that all the thread are initialized unsigned int iter = 0; // visual servo loop @@ -338,8 +332,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -348,13 +341,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } diff --git a/example/coin-simulator/simulateFourPoints2DPolarCamVelocity.cpp b/example/coin-simulator/simulateFourPoints2DPolarCamVelocity.cpp index a8c6d7a3a8..637b1adc97 100644 --- a/example/coin-simulator/simulateFourPoints2DPolarCamVelocity.cpp +++ b/example/coin-simulator/simulateFourPoints2DPolarCamVelocity.cpp @@ -122,8 +122,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - bool &display) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &display) { const char *optarg; int c; @@ -204,23 +203,21 @@ static void *mainLoop(void *_simu) // Project : computes the point coordinates in the camera frame and its 2D // coordinates for (int i = 0; i < 4; i++) { - point[i].changeFrame( - cMo); // Compute point coordinates in the camera frame - point[i] - .project(); // Compute desired point doordinates in the camera frame + point[i].changeFrame(cMo); // Compute point coordinates in the camera frame + point[i].project(); // Compute desired point doordinates in the camera frame } // Sets the desired position of the point vpFeaturePointPolar p[4]; for (int i = 0; i < 4; i++) - vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the - // vpPoint structure to build the - // polar coordinates + vpFeatureBuilder::create(p[i], + point[i]); // retrieve x,y and Z of the + // vpPoint structure to build the + // polar coordinates std::cout << "s: \n"; for (int i = 0; i < 4; i++) { - printf("[%d] rho %f theta %f Z %f\n", i, p[i].get_rho(), p[i].get_theta(), - p[i].get_Z()); + printf("[%d] rho %f theta %f Z %f\n", i, p[i].get_rho(), p[i].get_theta(), p[i].get_Z()); } // Sets the desired position of the point @@ -240,10 +237,8 @@ static void *mainLoop(void *_simu) pointd[2].setWorldCoordinates(0.1, 0.1, 0); pointd[3].setWorldCoordinates(-0.1, 0.1, 0); for (int i = 0; i < 4; i++) { - pointd[i].changeFrame( - cMod); // Compute desired point doordinates in the camera frame - pointd[i] - .project(); // Compute desired point doordinates in the camera frame + pointd[i].changeFrame(cMod); // Compute desired point doordinates in the camera frame + pointd[i].project(); // Compute desired point doordinates in the camera frame vpFeatureBuilder::create(pd[i], pointd[i]); // retrieve x,y and Z of the // vpPoint structure to build @@ -251,8 +246,7 @@ static void *mainLoop(void *_simu) } std::cout << "s*: \n"; for (int i = 0; i < 4; i++) { - printf("[%d] rho %f theta %f Z %f\n", i, pd[i].get_rho(), - pd[i].get_theta(), pd[i].get_Z()); + printf("[%d] rho %f theta %f Z %f\n", i, pd[i].get_rho(), pd[i].get_theta(), pd[i].get_Z()); } // Define the task @@ -360,8 +354,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -370,13 +363,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } diff --git a/example/device/display/displayD3D.cpp b/example/device/display/displayD3D.cpp index dad8d8b48e..d63744c46e 100644 --- a/example/device/display/displayD3D.cpp +++ b/example/device/display/displayD3D.cpp @@ -82,8 +82,7 @@ */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string opath, std::string user) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user) { fprintf(stdout, "\n\ Read an image on the disk, display it using D3D, display some\n\ @@ -143,8 +142,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, bool &click_allowed, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, bool &click_allowed, const std::string &user, bool &display) { const char *optarg; @@ -215,8 +213,7 @@ int main(int argc, const char **argv) vpIoTools::getUserName(username); // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, - username, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, username, opt_display) == false) { exit(-1); } @@ -249,8 +246,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -259,13 +255,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, opath, username); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -349,8 +342,7 @@ int main(int argc, const char **argv) // Display in overlay a yellow string ip.set_i(85); ip.set_j(100); - vpDisplay::displayText(I, ip, "ViSP is a marvelous software", - vpColor::yellow); + vpDisplay::displayText(I, ip, "ViSP is a marvelous software", vpColor::yellow); // Flush the display vpDisplay::flush(I); @@ -361,8 +353,7 @@ int main(int argc, const char **argv) vpDisplay::getImage(I, Ioverlay); // Write the color image on the disk - filename = - vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm"); + filename = vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm"); vpImageIo::write(Ioverlay, filename); // If click is allowed, wait for a mouse click to close the display @@ -425,8 +416,7 @@ int main(int argc, const char **argv) vpDisplay::getImage(Irgba, Ioverlay); // Write the color image on the disk - filename = - vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm"); + filename = vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm"); vpImageIo::write(Ioverlay, filename); // If click is allowed, wait for a blocking mouse click to exit. diff --git a/example/device/display/displayGDI.cpp b/example/device/display/displayGDI.cpp index 25f0a747c6..66f9c05e72 100644 --- a/example/device/display/displayGDI.cpp +++ b/example/device/display/displayGDI.cpp @@ -81,8 +81,7 @@ */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string opath, std::string user) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user) { fprintf(stdout, "\n\ Read an image on the disk, display it using GDI, display some\n\ @@ -141,8 +140,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, bool &click_allowed, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, bool &click_allowed, const std::string &user, bool &display) { const char *optarg; @@ -213,8 +211,7 @@ int main(int argc, const char **argv) vpIoTools::getUserName(username); // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, - username, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, username, opt_display) == false) { exit(-1); } @@ -247,8 +244,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -257,13 +253,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, opath, username); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -347,8 +340,7 @@ int main(int argc, const char **argv) // Display in overlay a yellow string ip.set_i(85); ip.set_j(100); - vpDisplay::displayText(I, ip, "ViSP is a marvelous software", - vpColor::yellow); + vpDisplay::displayText(I, ip, "ViSP is a marvelous software", vpColor::yellow); // Flush the display vpDisplay::flush(I); @@ -359,8 +351,7 @@ int main(int argc, const char **argv) vpDisplay::getImage(I, Ioverlay); // Write the color image on the disk - filename = - vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm"); + filename = vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm"); vpImageIo::write(Ioverlay, filename); // If click is allowed, wait for a mouse click to close the display @@ -423,8 +414,7 @@ int main(int argc, const char **argv) vpDisplay::getImage(Irgba, Ioverlay); // Write the color image on the disk - filename = - vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm"); + filename = vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm"); vpImageIo::write(Ioverlay, filename); // If click is allowed, wait for a blocking mouse click to exit. diff --git a/example/device/display/displayGTK.cpp b/example/device/display/displayGTK.cpp index cce6cf6b0c..66eaab574a 100644 --- a/example/device/display/displayGTK.cpp +++ b/example/device/display/displayGTK.cpp @@ -83,8 +83,7 @@ \param user : Username. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string opath, std::string user) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user) { fprintf(stdout, "\n\ Read an image on the disk, display it using GTK, display some\n\ @@ -145,8 +144,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, bool &click_allowed, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, bool &click_allowed, const std::string &user, bool &display) { const char *optarg; @@ -211,8 +209,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Set the default output path -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX opt_opath = "/tmp"; #elif defined(_WIN32) opt_opath = "C:\\temp"; @@ -222,8 +219,7 @@ int main(int argc, const char **argv) vpIoTools::getUserName(username); // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, - username, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, username, opt_display) == false) { exit(-1); } @@ -256,8 +252,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -266,13 +261,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, opath, username); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -356,8 +348,7 @@ int main(int argc, const char **argv) // Display in overlay a yellow string ip.set_i(85); ip.set_j(100); - vpDisplay::displayText(I, ip, "ViSP is a marvelous software", - vpColor::yellow); + vpDisplay::displayText(I, ip, "ViSP is a marvelous software", vpColor::yellow); // Flush the display vpDisplay::flush(I); @@ -368,8 +359,7 @@ int main(int argc, const char **argv) vpDisplay::getImage(I, Ioverlay); // Write the color image on the disk - filename = - vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm"); + filename = vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm"); vpImageIo::write(Ioverlay, filename); // If click is allowed, wait for a mouse click to close the display @@ -432,8 +422,7 @@ int main(int argc, const char **argv) vpDisplay::getImage(Irgba, Ioverlay); // Write the color image on the disk - filename = - vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm"); + filename = vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm"); vpImageIo::write(Ioverlay, filename); // If click is allowed, wait for a blocking mouse click to exit. @@ -450,9 +439,6 @@ int main(int argc, const char **argv) } } #else -int main() -{ - vpERROR_TRACE("You do not have GTK functionalities to display images..."); -} +int main() { vpERROR_TRACE("You do not have GTK functionalities to display images..."); } #endif diff --git a/example/device/display/displayOpenCV.cpp b/example/device/display/displayOpenCV.cpp index a06788a6b7..a1970d2dff 100644 --- a/example/device/display/displayOpenCV.cpp +++ b/example/device/display/displayOpenCV.cpp @@ -81,8 +81,7 @@ \param user : Username. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string opath, std::string user) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user) { fprintf(stdout, "\n\ Read an image on the disk, display it using OpenCV, display some\n\ @@ -146,8 +145,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, bool &click_allowed, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, bool &click_allowed, const std::string &user, bool &display) { const char *optarg_; @@ -212,8 +210,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Set the default output path -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX opt_opath = "/tmp"; #elif defined(_WIN32) opt_opath = "C:\\temp"; @@ -223,8 +220,7 @@ int main(int argc, const char **argv) vpIoTools::getUserName(username); // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, - username, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, username, opt_display) == false) { exit(-1); } @@ -257,8 +253,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -267,13 +262,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, opath, username); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -357,8 +349,7 @@ int main(int argc, const char **argv) // Display in overlay a yellow string ip.set_i(85); ip.set_j(100); - vpDisplay::displayText(I, ip, "ViSP is a marvelous software", - vpColor::yellow); + vpDisplay::displayText(I, ip, "ViSP is a marvelous software", vpColor::yellow); // Flush the display vpDisplay::flush(I); @@ -369,8 +360,7 @@ int main(int argc, const char **argv) vpDisplay::getImage(I, Ioverlay); // Write the color image on the disk - filename = - vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm"); + filename = vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm"); vpImageIo::write(Ioverlay, filename); // If click is allowed, wait for a mouse click to close the display @@ -433,8 +423,7 @@ int main(int argc, const char **argv) vpDisplay::getImage(Irgba, Ioverlay); // Write the color image on the disk - filename = - vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm"); + filename = vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm"); vpImageIo::write(Ioverlay, filename); // If click is allowed, wait for a blocking mouse click to exit. @@ -451,10 +440,6 @@ int main(int argc, const char **argv) } } #else -int main() -{ - vpERROR_TRACE( - "You do not have OpenCV functionalities to display images..."); -} +int main() { vpERROR_TRACE("You do not have OpenCV functionalities to display images..."); } #endif diff --git a/example/device/display/displaySequence.cpp b/example/device/display/displaySequence.cpp index b9fda307a7..309c1c33a7 100644 --- a/example/device/display/displaySequence.cpp +++ b/example/device/display/displaySequence.cpp @@ -55,8 +55,7 @@ #include #include #include -#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || \ - defined(VISP_HAVE_GDI)) +#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) #include #include @@ -80,11 +79,9 @@ // List of allowed command line options #define GETOPTARGS "di:p:hf:n:s:w" -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath, unsigned first, unsigned nimages, - unsigned step); -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &ppath, unsigned &first, unsigned &nimages, +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first, + unsigned nimages, unsigned step); +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages, unsigned &step, bool &display, bool &wait); /*! @@ -100,8 +97,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, \param step : Step between two images. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath, unsigned first, unsigned nimages, unsigned step) +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first, + unsigned nimages, unsigned step) { fprintf(stdout, "\n\ Read an image sequence from the disk and display it.\n\ @@ -181,8 +178,7 @@ SYNOPSIS\n\ \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, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages, unsigned &step, bool &display, bool &wait) { const char *optarg_; @@ -258,8 +254,8 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, - opt_step, opt_display, opt_wait) == false) { + if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_display, opt_wait) == + false) { exit(-1); } @@ -276,27 +272,21 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " 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); + usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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 option if you want to " - << std::endl - << " use personal images." << std::endl - << std::endl; + std::cerr << " Use -i 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 option if you want to " << std::endl + << " use personal images." << std::endl + << std::endl; exit(-1); } @@ -353,8 +343,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable" << std::endl; exit(-1); } @@ -428,9 +417,6 @@ int main(int argc, const char **argv) } } #else -int main() -{ - vpERROR_TRACE("You do not have X11 or GTK display functionalities..."); -} +int main() { vpERROR_TRACE("You do not have X11 or GTK display functionalities..."); } #endif diff --git a/example/device/display/displayX.cpp b/example/device/display/displayX.cpp index 15d497b9e8..4d49be0999 100644 --- a/example/device/display/displayX.cpp +++ b/example/device/display/displayX.cpp @@ -81,8 +81,7 @@ \param user : Username. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string opath, std::string user) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user) { fprintf(stdout, "\n\ Read an image on the disk, display it using X11, display some\n\ @@ -146,8 +145,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, bool &click_allowed, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, bool &click_allowed, const std::string &user, bool &display) { const char *optarg_; @@ -212,8 +210,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Set the default output path -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX opt_opath = "/tmp"; #elif defined(_WIN32) opt_opath = "C:\\temp"; @@ -223,8 +220,7 @@ int main(int argc, const char **argv) vpIoTools::getUserName(username); // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, - username, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, username, opt_display) == false) { exit(-1); } @@ -257,8 +253,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -267,13 +262,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, opath, username); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -357,8 +349,7 @@ int main(int argc, const char **argv) // Display in overlay a yellow string ip.set_i(85); ip.set_j(100); - vpDisplay::displayText(I, ip, "ViSP is a marvelous software", - vpColor::yellow); + vpDisplay::displayText(I, ip, "ViSP is a marvelous software", vpColor::yellow); // Flush the display vpDisplay::flush(I); @@ -369,8 +360,7 @@ int main(int argc, const char **argv) vpDisplay::getImage(I, Ioverlay); // Write the color image on the disk - filename = - vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm"); + filename = vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm"); vpImageIo::write(Ioverlay, filename); // If click is allowed, wait for a mouse click to close the display @@ -433,8 +423,7 @@ int main(int argc, const char **argv) vpDisplay::getImage(Irgba, Ioverlay); // Write the color image on the disk - filename = - vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm"); + filename = vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm"); vpImageIo::write(Ioverlay, filename); // If click is allowed, wait for a blocking mouse click to exit. @@ -451,9 +440,6 @@ int main(int argc, const char **argv) } } #else -int main() -{ - vpERROR_TRACE("You do not have X11 functionalities to display images..."); -} +int main() { vpERROR_TRACE("You do not have X11 functionalities to display images..."); } #endif diff --git a/example/device/display/displayXMulti.cpp b/example/device/display/displayXMulti.cpp index 52db7ca4c0..029bc3fbb7 100644 --- a/example/device/display/displayXMulti.cpp +++ b/example/device/display/displayXMulti.cpp @@ -84,8 +84,7 @@ \param user : Username. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string opath, std::string user) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user) { fprintf(stdout, "\n\ Read an image on the disk, display it using X11, display some\n\ @@ -148,8 +147,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, bool &click_allowed, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, bool &click_allowed, const std::string &user, bool &display) { const char *optarg_; @@ -214,8 +212,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Set the default output path -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX opt_opath = "/tmp"; #elif defined(_WIN32) opt_opath = "C:\\temp"; @@ -225,8 +222,7 @@ int main(int argc, const char **argv) vpIoTools::getUserName(username); // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, - username, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, username, opt_display) == false) { exit(-1); } @@ -259,8 +255,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -269,13 +264,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, opath, username); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -291,8 +283,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } try { @@ -303,8 +294,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -358,8 +348,7 @@ int main(int argc, const char **argv) // In the first display, display in overlay a yellow string ip.set_i(100); ip.set_j(100); - vpDisplay::displayText(I1, ip, "ViSP is a marvelous software", - vpColor::blue); + vpDisplay::displayText(I1, ip, "ViSP is a marvelous software", vpColor::blue); // Flush displays. The displays must be flushed to show the overlay. // without this line, nothing will be displayed. @@ -369,8 +358,7 @@ int main(int argc, const char **argv) // If click is allowed, wait for a blocking mouse click in the first // display, to display a cross at the clicked pixel position if (opt_click_allowed) { - std::cout << "\nA click in the first display to draw a cross..." - << std::endl; + std::cout << "\nA click in the first display to draw a cross..." << std::endl; // Blocking wait for a click. Get the position of the selected pixel // (i correspond to the row and j to the column coordinates in the // image) @@ -395,8 +383,7 @@ int main(int argc, const char **argv) vpDisplay::getImage(I1, Ioverlay); // Write the color image on the disk - filename = - vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm"); + filename = vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm"); vpImageIo::write(Ioverlay, filename); // If click is allowed, wait for a mouse click to close the display @@ -415,9 +402,6 @@ int main(int argc, const char **argv) } } #else -int main() -{ - vpERROR_TRACE("You do not have X11 functionalities to display images..."); -} +int main() { vpERROR_TRACE("You do not have X11 functionalities to display images..."); } #endif diff --git a/example/device/framegrabber/grab1394CMU.cpp b/example/device/framegrabber/grab1394CMU.cpp index f965013df1..025681aaea 100644 --- a/example/device/framegrabber/grab1394CMU.cpp +++ b/example/device/framegrabber/grab1394CMU.cpp @@ -60,10 +60,8 @@ // List of allowed command line options #define GETOPTARGS "dhn:o:" -void usage(const char *name, const char *badparam, unsigned &nframes, - std::string &opath); -bool getOptions(int argc, const char **argv, bool &display, - unsigned int &nframes, bool &save, std::string &opath); +void usage(const char *name, const char *badparam, unsigned &nframes, std::string &opath); +bool getOptions(int argc, const char **argv, bool &display, unsigned int &nframes, bool &save, std::string &opath); /*! @@ -75,8 +73,7 @@ bool getOptions(int argc, const char **argv, bool &display, \param opath : Image filename when saving. */ -void usage(const char *name, const char *badparam, unsigned &nframes, - std::string &opath) +void usage(const char *name, const char *badparam, unsigned &nframes, std::string &opath) { fprintf(stdout, "\n\ Acquire images using CMU 1394 Digital Camera SDK (available under Windows only) and display\n\ @@ -122,8 +119,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &display, - unsigned int &nframes, bool &save, std::string &opath) +bool getOptions(int argc, const char **argv, bool &display, unsigned int &nframes, bool &save, std::string &opath) { const char *optarg_; int c; @@ -202,19 +198,16 @@ int main(int argc, const char **argv) vp1394CMUGrabber g; unsigned short gain_min, gain_max; g.getGainMinMax(gain_min, gain_max); - std::cout << "Gain range [" << gain_min << ", " << gain_max << "]" - << std::endl; + std::cout << "Gain range [" << gain_min << ", " << gain_max << "]" << std::endl; unsigned short shutter_min, shutter_max; g.getShutterMinMax(shutter_min, shutter_max); - std::cout << "Shutter range [" << shutter_min << ", " << shutter_max << "]" - << std::endl; + std::cout << "Shutter range [" << shutter_min << ", " << shutter_max << "]" << std::endl; g.setFramerate(4); // 30 fps std::cout << "Actual framerate: " << g.getFramerate() << std::endl; g.setVideoMode(0, 0); g.acquire(I); - std::cout << "Image size: width : " << I.getWidth() - << " height: " << I.getHeight() << std::endl; + std::cout << "Image size: width : " << I.getWidth() << " height: " << I.getHeight() << std::endl; #if (defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) @@ -263,8 +256,7 @@ int main(int argc, const char **argv) ttotal += tloop; } std::cout << "Mean loop time: " << ttotal / nframes << " ms" << std::endl; - std::cout << "Mean frequency: " << 1000. / (ttotal / nframes) << " fps" - << std::endl; + std::cout << "Mean frequency: " << 1000. / (ttotal / nframes) << " fps" << std::endl; } catch (vpException &e) { std::cout << "Catch an exception: " << e << std::endl; return 1; @@ -273,8 +265,7 @@ int main(int argc, const char **argv) #else int main() { - std::cout << "This example requires CMU 1394 Digital Camera SDK. " - << std::endl; + std::cout << "This example requires CMU 1394 Digital Camera SDK. " << std::endl; return 0; } #endif diff --git a/example/device/framegrabber/grab1394Two.cpp b/example/device/framegrabber/grab1394Two.cpp index f3ee0fa2e5..7604d723f8 100644 --- a/example/device/framegrabber/grab1394Two.cpp +++ b/example/device/framegrabber/grab1394Two.cpp @@ -73,27 +73,18 @@ #define GETOPTARGS "b:c:df:g:hH:L:mn:io:p:rsT:v:W:" #define DUAL_ACQ -void usage(const char *name, const char *badparam, unsigned int camera, - const unsigned int &nframes, const std::string &opath, - const unsigned int &roi_left, const unsigned int &roi_top, - const unsigned int &roi_width, const unsigned int &roi_height, - const unsigned int &ringbuffersize, +void usage(const char *name, const char *badparam, unsigned int camera, const unsigned int &nframes, + const std::string &opath, const unsigned int &roi_left, const unsigned int &roi_top, + const unsigned int &roi_width, const unsigned int &roi_height, const unsigned int &ringbuffersize, const unsigned int &panControl); -void read_options(int argc, const char **argv, bool &multi, - unsigned int &camera, unsigned int &nframes, - bool &verbose_info, bool &verbose_settings, - bool &videomode_is_set, - vp1394TwoGrabber::vp1394TwoVideoModeType &videomode, - bool &framerate_is_set, - vp1394TwoGrabber::vp1394TwoFramerateType &framerate, - bool &colorcoding_is_set, - vp1394TwoGrabber::vp1394TwoColorCodingType &colorcoding, - bool &ringbuffersize_is_set, unsigned int &ringbuffersize, - bool &display, bool &save, std::string &opath, - unsigned int &roi_left, unsigned int &roi_top, - unsigned int &roi_width, unsigned int &roi_height, - bool &reset, unsigned int &panControl, - bool &panControl_is_set); +void read_options(int argc, const char **argv, bool &multi, unsigned int &camera, unsigned int &nframes, + bool &verbose_info, bool &verbose_settings, bool &videomode_is_set, + vp1394TwoGrabber::vp1394TwoVideoModeType &videomode, bool &framerate_is_set, + vp1394TwoGrabber::vp1394TwoFramerateType &framerate, bool &colorcoding_is_set, + vp1394TwoGrabber::vp1394TwoColorCodingType &colorcoding, bool &ringbuffersize_is_set, + unsigned int &ringbuffersize, bool &display, bool &save, std::string &opath, unsigned int &roi_left, + unsigned int &roi_top, unsigned int &roi_width, unsigned int &roi_height, bool &reset, + unsigned int &panControl, bool &panControl_is_set); /*! @@ -110,11 +101,10 @@ void read_options(int argc, const char **argv, bool &multi, \param panControl : Pan control value. */ -void usage(const char *name, const char *badparam, unsigned int camera, - const unsigned int &nframes, const std::string &opath, - const unsigned int &roi_left, const unsigned int &roi_top, - const unsigned int &roi_width, const unsigned int &roi_height, - const unsigned int &ringbuffersize, const unsigned int &panControl) +void usage(const char *name, const char *badparam, unsigned int camera, const unsigned int &nframes, + const std::string &opath, const unsigned int &roi_left, const unsigned int &roi_top, + const unsigned int &roi_width, const unsigned int &roi_height, const unsigned int &ringbuffersize, + const unsigned int &panControl) { if (badparam) fprintf(stderr, "\nERROR: Bad parameter [%s]\n", badparam); @@ -221,9 +211,8 @@ OPTIONS Default\n\ program was not properly stopped by a CTRL-C.\n\ \n\ -h : Print this help.\n\ - \n", name, name, name, name, name, name, name, name, name, - roi_left, roi_top, roi_width, roi_height, camera, nframes, - ringbuffersize, panControl, opath.c_str()); + \n", name, name, name, name, name, name, name, name, name, roi_left, roi_top, roi_width, roi_height, + camera, nframes, ringbuffersize, panControl, opath.c_str()); } /*! @@ -268,21 +257,14 @@ OPTIONS Default\n\ has to be set. */ -void read_options(int argc, const char **argv, bool &multi, - unsigned int &camera, unsigned int &nframes, - bool &verbose_info, bool &verbose_settings, - bool &videomode_is_set, - vp1394TwoGrabber::vp1394TwoVideoModeType &videomode, - bool &framerate_is_set, - vp1394TwoGrabber::vp1394TwoFramerateType &framerate, - bool &colorcoding_is_set, - vp1394TwoGrabber::vp1394TwoColorCodingType &colorcoding, - bool &ringbuffersize_is_set, unsigned int &ringbuffersize, - bool &display, bool &save, std::string &opath, - unsigned int &roi_left, unsigned int &roi_top, - unsigned int &roi_width, unsigned int &roi_height, - bool &reset, unsigned int &panControl, - bool &panControl_is_set) +void read_options(int argc, const char **argv, bool &multi, unsigned int &camera, unsigned int &nframes, + bool &verbose_info, bool &verbose_settings, bool &videomode_is_set, + vp1394TwoGrabber::vp1394TwoVideoModeType &videomode, bool &framerate_is_set, + vp1394TwoGrabber::vp1394TwoFramerateType &framerate, bool &colorcoding_is_set, + vp1394TwoGrabber::vp1394TwoColorCodingType &colorcoding, bool &ringbuffersize_is_set, + unsigned int &ringbuffersize, bool &display, bool &save, std::string &opath, unsigned int &roi_left, + unsigned int &roi_top, unsigned int &roi_width, unsigned int &roi_height, bool &reset, + unsigned int &panControl, bool &panControl_is_set) { /* * Lecture des options. @@ -351,8 +333,8 @@ void read_options(int argc, const char **argv, bool &multi, break; case 'h': case '?': - usage(argv[0], NULL, camera, nframes, opath, roi_left, roi_top, - roi_width, roi_height, ringbuffersize, panControl); + usage(argv[0], NULL, camera, nframes, opath, roi_left, roi_top, roi_width, roi_height, ringbuffersize, + panControl); exit(0); break; } @@ -360,8 +342,7 @@ void read_options(int argc, const char **argv, bool &multi, if ((c == 1) || (c == -1)) { // standalone param or error - usage(argv[0], NULL, camera, nframes, opath, roi_left, roi_top, roi_width, - roi_height, ringbuffersize, panControl); + usage(argv[0], NULL, camera, nframes, opath, roi_left, roi_top, roi_width, roi_height, ringbuffersize, panControl); std::cerr << "ERROR: " << std::endl; std::cerr << " Bad argument " << optarg_ << std::endl << std::endl; exit(-1); @@ -406,12 +387,9 @@ int main(int argc, const char **argv) // Default output path for image saving std::string opath = "/tmp/I%d-%04d.ppm"; - read_options(argc, argv, multi, camera, nframes, verbose_info, - verbose_settings, videomode_is_set, videomode, - framerate_is_set, framerate, colorcoding_is_set, colorcoding, - ringbuffersize_is_set, ringbuffersize, display, save, opath, - roi_left, roi_top, roi_width, roi_height, reset, panControl, - panControl_is_set); + read_options(argc, argv, multi, camera, nframes, verbose_info, verbose_settings, videomode_is_set, videomode, + framerate_is_set, framerate, colorcoding_is_set, colorcoding, ringbuffersize_is_set, ringbuffersize, + display, save, opath, roi_left, roi_top, roi_width, roi_height, reset, panControl, panControl_is_set); // Create a grabber vp1394TwoGrabber g(reset); @@ -432,12 +410,9 @@ int main(int argc, const char **argv) if (multi) { // ckeck if two cameras are connected if (ncameras < 2) { - std::cout << "You have only " << ncameras - << " camera connected on the bus." << std::endl; - std::cout << "It is not possible to active multi-camera acquisition." - << std::endl; - std::cout << "Disable -m command line option, or connect an other " - << std::endl; + std::cout << "You have only " << ncameras << " camera connected on the bus." << std::endl; + std::cout << "It is not possible to active multi-camera acquisition." << std::endl; + std::cout << "Disable -m command line option, or connect an other " << std::endl; std::cout << "cameras on the bus." << std::endl; g.close(); return (0); @@ -446,8 +421,7 @@ int main(int argc, const char **argv) if (camera >= ncameras) { std::cout << "You have only " << ncameras; std::cout << " camera connected on the bus." << std::endl; - std::cout << "It is not possible to select camera " << camera - << std::endl; + std::cout << "It is not possible to select camera " << camera << std::endl; std::cout << "Check your -c command line option." << std::endl; g.close(); return (0); @@ -479,12 +453,9 @@ int main(int argc, const char **argv) std::list lmode; std::list lfps; std::list lcoding; - std::list::const_iterator - it_lmode; - std::list::const_iterator - it_lfps; - std::list:: - const_iterator it_lcoding; + std::list::const_iterator it_lmode; + std::list::const_iterator it_lfps; + std::list::const_iterator it_lcoding; uint64_t guid; g.getVideoMode(curmode); @@ -493,51 +464,37 @@ int main(int argc, const char **argv) g.getVideoModeSupported(lmode); g.getGuid(guid); - std::cout - << "----------------------------------------------------------" - << std::endl - << "---- Video modes and framerates supported by camera " - << i + offset << " ----" << std::endl - << "---- with guid 0x" << std::hex << guid - << " ----" << std::endl - << "---- * is for the current settings ----" - << std::endl - << "---- between ( ) you have the corresponding option ----" - << std::endl - << "---- to use. ----" - << std::endl - << "----------------------------------------------------------" - << std::endl; - - for (it_lmode = lmode.begin(); it_lmode != lmode.end(); - ++it_lmode) { + std::cout << "----------------------------------------------------------" << std::endl + << "---- Video modes and framerates supported by camera " << i + offset << " ----" << std::endl + << "---- with guid 0x" << std::hex << guid << " ----" << std::endl + << "---- * is for the current settings ----" << std::endl + << "---- between ( ) you have the corresponding option ----" << std::endl + << "---- to use. ----" << std::endl + << "----------------------------------------------------------" << std::endl; + + for (it_lmode = lmode.begin(); it_lmode != lmode.end(); ++it_lmode) { // Parse the list of supported modes vp1394TwoGrabber::vp1394TwoVideoModeType supmode = *it_lmode; if (curmode == supmode) - std::cout << " * " - << vp1394TwoGrabber::videoMode2string(supmode) - << " (-v " << (int)supmode << ")" << std::endl; + std::cout << " * " << vp1394TwoGrabber::videoMode2string(supmode) << " (-v " << (int)supmode << ")" + << std::endl; else - std::cout << " " - << vp1394TwoGrabber::videoMode2string(supmode) - << " (-v " << (int)supmode << ")" << std::endl; + std::cout << " " << vp1394TwoGrabber::videoMode2string(supmode) << " (-v " << (int)supmode << ")" + << std::endl; if (g.isVideoModeFormat7(supmode)) { // Format 7 video mode; no framerate setting, but color // coding setting g.getColorCodingSupported(supmode, lcoding); - for (it_lcoding = lcoding.begin(); it_lcoding != lcoding.end(); - ++it_lcoding) { + for (it_lcoding = lcoding.begin(); it_lcoding != lcoding.end(); ++it_lcoding) { vp1394TwoGrabber::vp1394TwoColorCodingType supcoding; supcoding = *it_lcoding; if ((curmode == supmode) && (supcoding == curcoding)) - std::cout << " * " - << vp1394TwoGrabber::colorCoding2string(supcoding) - << " (-g " << (int)supcoding << ")" << std::endl; + std::cout << " * " << vp1394TwoGrabber::colorCoding2string(supcoding) << " (-g " << (int)supcoding + << ")" << std::endl; else - std::cout << " " - << vp1394TwoGrabber::colorCoding2string(supcoding) - << " (-g " << (int)supcoding << ")" << std::endl; + std::cout << " " << vp1394TwoGrabber::colorCoding2string(supcoding) << " (-g " << (int)supcoding + << ")" << std::endl; } } else { @@ -546,19 +503,15 @@ int main(int argc, const char **argv) for (it_lfps = lfps.begin(); it_lfps != lfps.end(); ++it_lfps) { vp1394TwoGrabber::vp1394TwoFramerateType supfps = *it_lfps; if ((curmode == supmode) && (supfps == curfps)) - std::cout << " * " - << vp1394TwoGrabber::framerate2string(supfps) - << " (-f " << (int)supfps << ")" << std::endl; + std::cout << " * " << vp1394TwoGrabber::framerate2string(supfps) << " (-f " << (int)supfps << ")" + << std::endl; else - std::cout << " " - << vp1394TwoGrabber::framerate2string(supfps) - << " (-f " << (int)supfps << ")" << std::endl; + std::cout << " " << vp1394TwoGrabber::framerate2string(supfps) << " (-f " << (int)supfps << ")" + << std::endl; } } } - std::cout - << "----------------------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------------------" << std::endl; } } return 0; @@ -619,8 +572,7 @@ int main(int argc, const char **argv) // Acquire the first image if (grab_color[i]) { g.acquire(Ic[i]); - std::cout << "Image size for camera " << i + offset - << " : width: " << Ic[i].getWidth() + std::cout << "Image size for camera " << i + offset << " : width: " << Ic[i].getWidth() << " height: " << Ic[i].getHeight() << std::endl; #ifdef VISP_HAVE_X11 @@ -635,8 +587,7 @@ int main(int argc, const char **argv) #endif } else { g.acquire(Ig[i]); - std::cout << "Image size for camera " << i + offset - << " : width: " << Ig[i].getWidth() + std::cout << "Image size for camera " << i + offset << " : width: " << Ig[i].getWidth() << " height: " << Ig[i].getHeight() << std::endl; #ifdef VISP_HAVE_X11 @@ -703,8 +654,7 @@ int main(int argc, const char **argv) } std::cout << "Mean loop time: " << ttotal / nframes << " ms" << std::endl; - std::cout << "Mean frequency: " << 1000. / (ttotal / nframes) << " fps" - << std::endl; + std::cout << "Mean frequency: " << 1000. / (ttotal / nframes) << " fps" << std::endl; // Release the framegrabber g.close(); diff --git a/example/device/framegrabber/grabDirectShow.cpp b/example/device/framegrabber/grabDirectShow.cpp index ec75f0e007..63cfe061bb 100644 --- a/example/device/framegrabber/grabDirectShow.cpp +++ b/example/device/framegrabber/grabDirectShow.cpp @@ -72,8 +72,7 @@ \param opath : Image filename when saving. */ -void usage(const char *name, const char *badparam, unsigned &nframes, - std::string &opath) +void usage(const char *name, const char *badparam, unsigned &nframes, std::string &opath) { fprintf(stdout, "\n\ Acquire images using DirectShow (under Windows only) and display\n\ @@ -119,8 +118,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &display, unsigned &nframes, - bool &save, std::string &opath) +bool getOptions(int argc, const char **argv, bool &display, unsigned &nframes, bool &save, std::string &opath) { const char *optarg; int c; @@ -201,8 +199,7 @@ int main(int argc, const char **argv) // test if a camera is connected if (grabber->getDeviceNumber() == 0) { - vpCTRACE << "there is no camera detected on your computer." - << std::endl; + vpCTRACE << "there is no camera detected on your computer." << std::endl; grabber->close(); exit(0); } @@ -212,8 +209,7 @@ int main(int argc, const char **argv) // Acquire an image grabber->acquire(I); - std::cout << "Image size: width : " << I.getWidth() - << " height: " << I.getHeight() << std::endl; + std::cout << "Image size: width : " << I.getWidth() << " height: " << I.getHeight() << std::endl; // Creates a display #if defined VISP_HAVE_GTK @@ -255,8 +251,7 @@ int main(int argc, const char **argv) ttotal += tloop; } std::cout << "Mean loop time: " << ttotal / nframes << " ms" << std::endl; - std::cout << "Mean frequency: " << 1000. / (ttotal / nframes) << " fps" - << std::endl; + std::cout << "Mean frequency: " << 1000. / (ttotal / nframes) << " fps" << std::endl; // Release the framegrabber delete grabber; diff --git a/example/device/framegrabber/grabDirectShowMulti.cpp b/example/device/framegrabber/grabDirectShowMulti.cpp index d91b890c77..214bf7c85d 100644 --- a/example/device/framegrabber/grabDirectShowMulti.cpp +++ b/example/device/framegrabber/grabDirectShowMulti.cpp @@ -82,8 +82,7 @@ Print the program options. \param opath : Image filename when saving. */ -void usage(const char *name, const char *badparam, unsigned int camera, - unsigned int &nframes, std::string &opath) +void usage(const char *name, const char *badparam, unsigned int camera, unsigned int &nframes, std::string &opath) { if (badparam) fprintf(stderr, "\nERREUR: Bad parameter [%s]\n", badparam); @@ -172,12 +171,9 @@ Set the program options. */ -void read_options(int argc, const char **argv, bool &multi, - unsigned int &camera, unsigned int &nframes, - bool &verbose_info, bool &verbose_settings, - bool &mediatype_is_set, unsigned int &mediatypeID, - bool &framerate_is_set, double &framerate, bool &display, - bool &save, std::string &opath) +void read_options(int argc, const char **argv, bool &multi, unsigned int &camera, unsigned int &nframes, + bool &verbose_info, bool &verbose_settings, bool &mediatype_is_set, unsigned int &mediatypeID, + bool &framerate_is_set, double &framerate, bool &display, bool &save, std::string &opath) { const char *optarg; int c; @@ -266,8 +262,7 @@ int main(int argc, const char **argv) #elif defined VISP_HAVE_GTK vpDisplayGTK *d; #endif - read_options(argc, argv, multi, camera, nframes, verbose_info, - verbose_settings, mediatype_is_set, mediatypeID, + read_options(argc, argv, multi, camera, nframes, verbose_info, verbose_settings, mediatype_is_set, mediatypeID, framerate_is_set, framerate, display, save, opath); // Number of cameras connected on the bus @@ -278,12 +273,9 @@ int main(int argc, const char **argv) if (multi) { // ckeck if two cameras are connected if (ncameras < 2) { - std::cout << "You have only " << ncameras - << " camera connected on the bus." << std::endl; - std::cout << "It is not possible to active multi-camera acquisition." - << std::endl; - std::cout << "Disable -m command line option, or connect an other " - << std::endl; + std::cout << "You have only " << ncameras << " camera connected on the bus." << std::endl; + std::cout << "It is not possible to active multi-camera acquisition." << std::endl; + std::cout << "Disable -m command line option, or connect an other " << std::endl; std::cout << "cameras on the bus." << std::endl; g->close(); delete g; @@ -293,8 +285,7 @@ int main(int argc, const char **argv) if (camera >= ncameras) { std::cout << "You have only " << ncameras; std::cout << " camera connected on the bus." << std::endl; - std::cout << "It is not possible to select camera " << camera - << std::endl; + std::cout << "It is not possible to select camera " << camera << std::endl; std::cout << "Check your -c command line option." << std::endl; g->close(); delete g; @@ -353,13 +344,9 @@ int main(int argc, const char **argv) // Display information for each camera if (verbose_info || verbose_settings) { - std::cout - << "----------------------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------------------" << std::endl; std::cout << "---- Device List : " << std::endl; - std::cout - << "----------------------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------------------" << std::endl; g[0].displayDevices(); for (unsigned i = 0; i < ncameras; i++) { unsigned int c; @@ -371,30 +358,20 @@ int main(int argc, const char **argv) if (verbose_info) { unsigned int width, height; g[i].getFormat(width, height, framerate); - std::cout - << "----------------------------------------------------------" - << std::endl - << "---- MediaType and framerate currently used by device " - << std::endl - << "---- (or camera) " << c << std::endl - << "---- Current MediaType : " << g[i].getMediaType() - << std::endl - << "---- Current format : " << width << " x " << height - << " at " << framerate << " fps" << std::endl - << "----------------------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------------------" << std::endl + << "---- MediaType and framerate currently used by device " << std::endl + << "---- (or camera) " << c << std::endl + << "---- Current MediaType : " << g[i].getMediaType() << std::endl + << "---- Current format : " << width << " x " << height << " at " << framerate << " fps" + << std::endl + << "----------------------------------------------------------" << std::endl; } if (verbose_settings) { - std::cout - << "----------------------------------------------------------" - << std::endl - << "---- MediaTypes supported by device (or camera) " << c - << std::endl - << "---- One of the MediaType below can be set using " - << std::endl - << "---- option -t ." << std::endl - << "----------------------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------------------" << std::endl + << "---- MediaTypes supported by device (or camera) " << c << std::endl + << "---- One of the MediaType below can be set using " << std::endl + << "---- option -t ." << std::endl + << "----------------------------------------------------------" << std::endl; g[i].getStreamCapabilities(); } } @@ -416,9 +393,8 @@ int main(int argc, const char **argv) else c = camera; - std::cout << "Image size for camera " << c - << " : width: " << I[i].getWidth() - << " height: " << I[i].getHeight() << std::endl; + std::cout << "Image size for camera " << c << " : width: " << I[i].getWidth() << " height: " << I[i].getHeight() + << std::endl; if (display) { // Initialise the display @@ -460,8 +436,7 @@ int main(int argc, const char **argv) } std::cout << "Mean loop time: " << ttotal / nframes << " ms" << std::endl; - std::cout << "Mean frequency: " << 1000. / (ttotal / nframes) << " fps" - << std::endl; + std::cout << "Mean frequency: " << 1000. / (ttotal / nframes) << " fps" << std::endl; // Release the framegrabber delete[] g; diff --git a/example/device/framegrabber/grabDisk.cpp b/example/device/framegrabber/grabDisk.cpp index 922e2ae440..f312734c5c 100644 --- a/example/device/framegrabber/grabDisk.cpp +++ b/example/device/framegrabber/grabDisk.cpp @@ -64,13 +64,10 @@ // List of allowed command line options #define GETOPTARGS "b:de:f:i:hn:s:z:" -void usage(const char *name, const char *badparam, std::string ipath, - std::string basename, std::string ext, int first, +void usage(const char *name, const char *badparam, std::string ipath, std::string basename, std::string ext, int first, unsigned int nimages, int step, unsigned int nzero); -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &basename, std::string &ext, int &first, - unsigned int &nimages, int &step, unsigned int &nzero, - bool &display); +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &basename, std::string &ext, int &first, + unsigned int &nimages, int &step, unsigned int &nzero, bool &display); /* @@ -87,8 +84,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, \param nzero : Number of zero for the image number coding. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string basename, std::string ext, int first, +void usage(const char *name, const char *badparam, std::string ipath, std::string basename, std::string ext, int first, unsigned int nimages, int step, unsigned int nzero) { fprintf(stdout, "\n\ @@ -140,8 +136,7 @@ OPTIONS: Default\n\ Turn off the display.\n\ \n\ -h \n\ - Print the help.\n\n", ipath.c_str(), basename.c_str(), ext.c_str(), first, - nimages, step, nzero); + Print the help.\n\n", ipath.c_str(), basename.c_str(), ext.c_str(), first, nimages, step, nzero); if (badparam) fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); @@ -164,10 +159,8 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &basename, std::string &ext, int &first, - unsigned int &nimages, int &step, unsigned int &nzero, - bool &display) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &basename, std::string &ext, int &first, + unsigned int &nimages, int &step, unsigned int &nzero, bool &display) { const char *optarg_; int c; @@ -204,8 +197,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, break; default: - usage(argv[0], optarg_, ipath, basename, ext, first, nimages, step, - nzero); + usage(argv[0], optarg_, ipath, basename, ext, first, nimages, step, nzero); return false; break; } @@ -255,8 +247,8 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_basename, opt_ext, opt_first, - opt_nimages, opt_step, opt_nzero, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_basename, opt_ext, opt_first, opt_nimages, opt_step, opt_nzero, + opt_display) == false) { exit(-1); } @@ -270,24 +262,19 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " 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_basename, opt_ext, opt_first, - opt_nimages, opt_step, opt_nzero); + usage(argv[0], NULL, ipath, opt_basename, opt_ext, opt_first, opt_nimages, opt_step, opt_nzero); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -317,8 +304,7 @@ int main(int argc, const char **argv) // Open the framegrabber by loading the first image of the sequence g.open(I); - std::cout << "Image size: width : " << I.getWidth() - << " height: " << I.getHeight() << std::endl; + std::cout << "Image size: width : " << I.getWidth() << " height: " << I.getHeight() << std::endl; // We open a window using either X11 or GDI. // Its size is automatically defined by the image (I) size @@ -367,9 +353,6 @@ int main(int argc, const char **argv) } #else -int main() -{ - vpERROR_TRACE("You do not have X11 or GTK display functionalities..."); -} +int main() { vpERROR_TRACE("You do not have X11 or GTK display functionalities..."); } #endif diff --git a/example/device/framegrabber/grabFlyCapture.cpp b/example/device/framegrabber/grabFlyCapture.cpp index 392e943294..c57175d742 100644 --- a/example/device/framegrabber/grabFlyCapture.cpp +++ b/example/device/framegrabber/grabFlyCapture.cpp @@ -72,8 +72,7 @@ \param opath : Image filename when saving. */ -void usage(const char *name, const char *badparam, unsigned int icamera, - std::string &opath) +void usage(const char *name, const char *badparam, unsigned int icamera, std::string &opath) { fprintf(stdout, "\n\ Acquire and display images using PointGrey FlyCapture SDK.\n\ @@ -123,8 +122,8 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &display, bool &click, - bool &save, std::string &opath, unsigned int &icamera) +bool getOptions(int argc, const char **argv, bool &display, bool &click, bool &save, std::string &opath, + unsigned int &icamera) { const char *optarg_; int c; @@ -182,8 +181,7 @@ int main(int argc, const char **argv) vpImage I; // for gray images // Read the command line options - if (getOptions(argc, argv, opt_display, opt_click, opt_save, opt_opath, - opt_icamera) == false) { + if (getOptions(argc, argv, opt_display, opt_click, opt_save, opt_opath, opt_icamera) == false) { return 0; } @@ -191,10 +189,8 @@ int main(int argc, const char **argv) vpFlyCaptureGrabber g; g.setCameraIndex(opt_icamera); // open the default camera g.open(I); - std::cout << "Camera serial: " << g.getCameraSerial(g.getCameraIndex()) - << std::endl; - std::cout << "Image size : " << I.getWidth() << " " << I.getHeight() - << std::endl; + std::cout << "Camera serial: " << g.getCameraSerial(g.getCameraIndex()) << std::endl; + std::cout << "Image size : " << I.getWidth() << " " << I.getHeight() << std::endl; vpDisplay *display = NULL; if (opt_display) { @@ -246,9 +242,6 @@ int main(int argc, const char **argv) } #else -int main() -{ - std::cout << "PointGrey FlyCapture SDK is not available..." << std::endl; -} +int main() { std::cout << "PointGrey FlyCapture SDK is not available..." << std::endl; } #endif diff --git a/example/device/framegrabber/grabOpenCV.cpp b/example/device/framegrabber/grabOpenCV.cpp index 424d02e9e8..7ee7430e57 100644 --- a/example/device/framegrabber/grabOpenCV.cpp +++ b/example/device/framegrabber/grabOpenCV.cpp @@ -84,11 +84,9 @@ int main(int argc, char **argv) std::cout << "Image size: " #if (VISP_HAVE_OPENCV_VERSION >= 0x030000) - << (int)cap.get(cv::CAP_PROP_FRAME_WIDTH) << " " - << (int)cap.get(cv::CAP_PROP_FRAME_HEIGHT) << std::endl; + << (int)cap.get(cv::CAP_PROP_FRAME_WIDTH) << " " << (int)cap.get(cv::CAP_PROP_FRAME_HEIGHT) << std::endl; #else - << (int)cap.get(CV_CAP_PROP_FRAME_WIDTH) << " " - << (int)cap.get(CV_CAP_PROP_FRAME_HEIGHT) << std::endl; + << (int)cap.get(CV_CAP_PROP_FRAME_WIDTH) << " " << (int)cap.get(CV_CAP_PROP_FRAME_HEIGHT) << std::endl; #endif // vpImage I; // for color images diff --git a/example/device/framegrabber/grabRealSense.cpp b/example/device/framegrabber/grabRealSense.cpp index f3ad2885a3..f2eeb58af3 100644 --- a/example/device/framegrabber/grabRealSense.cpp +++ b/example/device/framegrabber/grabRealSense.cpp @@ -69,23 +69,16 @@ #ifdef VISP_HAVE_PCL #ifdef USE_THREAD // Shared vars -typedef enum { - capture_waiting, - capture_started, - capture_stopped -} t_CaptureState; +typedef enum { capture_waiting, capture_started, capture_stopped } t_CaptureState; t_CaptureState s_capture_state = capture_waiting; vpMutex s_mutex_capture; vpThread::Return displayPointcloudFunction(vpThread::Args args) { - pcl::PointCloud::Ptr pointcloud_ = - *((pcl::PointCloud::Ptr *)args); + pcl::PointCloud::Ptr pointcloud_ = *((pcl::PointCloud::Ptr *)args); - pcl::visualization::PCLVisualizer::Ptr viewer( - new pcl::visualization::PCLVisualizer("3D Viewer")); - pcl::visualization::PointCloudColorHandlerRGBField rgb( - pointcloud_); + pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); + pcl::visualization::PointCloudColorHandlerRGBField rgb(pointcloud_); viewer->setBackgroundColor(0, 0, 0); viewer->initCameraParameters(); viewer->setPosition(640 + 80, 480 + 80); @@ -102,14 +95,11 @@ vpThread::Return displayPointcloudFunction(vpThread::Args args) if (capture_state_ == capture_started) { static bool update = false; if (!update) { - viewer->addPointCloud(pointcloud_, rgb, - "sample cloud"); - viewer->setPointCloudRenderingProperties( - pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); + viewer->addPointCloud(pointcloud_, rgb, "sample cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); update = true; } else { - viewer->updatePointCloud(pointcloud_, rgb, - "sample cloud"); + viewer->updatePointCloud(pointcloud_, rgb, "sample cloud"); } viewer->spinOnce(10); @@ -128,62 +118,39 @@ int main() #if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) try { vpRealSense rs; - rs.setStreamSettings( - rs::stream::color, - vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30)); + rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30)); rs.open(); - std::cout << rs.getCameraParameters( - rs::stream::color, - vpCameraParameters::perspectiveProjWithoutDistortion) + std::cout << rs.getCameraParameters(rs::stream::color, vpCameraParameters::perspectiveProjWithoutDistortion) << std::endl; - std::cout << rs.getCameraParameters( - rs::stream::color, - vpCameraParameters::perspectiveProjWithDistortion) - << std::endl; - std::cout << "Extrinsics cMd: \n" - << rs.getTransformation(rs::stream::color, rs::stream::depth) - << std::endl; - std::cout << "Extrinsics dMc: \n" - << rs.getTransformation(rs::stream::depth, rs::stream::color) - << std::endl; - std::cout << "Extrinsics cMi: \n" - << rs.getTransformation(rs::stream::color, rs::stream::infrared) - << std::endl; - std::cout << "Extrinsics dMi: \n" - << rs.getTransformation(rs::stream::depth, rs::stream::infrared) + std::cout << rs.getCameraParameters(rs::stream::color, vpCameraParameters::perspectiveProjWithDistortion) << std::endl; + std::cout << "Extrinsics cMd: \n" << rs.getTransformation(rs::stream::color, rs::stream::depth) << std::endl; + std::cout << "Extrinsics dMc: \n" << rs.getTransformation(rs::stream::depth, rs::stream::color) << std::endl; + std::cout << "Extrinsics cMi: \n" << rs.getTransformation(rs::stream::color, rs::stream::infrared) << std::endl; + std::cout << "Extrinsics dMi: \n" << rs.getTransformation(rs::stream::depth, rs::stream::infrared) << std::endl; - vpImage color( - (unsigned int)rs.getIntrinsics(rs::stream::color).height, - (unsigned int)rs.getIntrinsics(rs::stream::color).width); + vpImage color((unsigned int)rs.getIntrinsics(rs::stream::color).height, + (unsigned int)rs.getIntrinsics(rs::stream::color).width); - vpImage infrared_display( - (unsigned int)rs.getIntrinsics(rs::stream::infrared).height, - (unsigned int)rs.getIntrinsics(rs::stream::infrared).width); + vpImage infrared_display((unsigned int)rs.getIntrinsics(rs::stream::infrared).height, + (unsigned int)rs.getIntrinsics(rs::stream::infrared).width); vpImage infrared_y16; rs::device *dev = rs.getHandler(); - bool use_infrared_y16 = - dev->get_stream_format(rs::stream::infrared) == rs::format::y16; + bool use_infrared_y16 = dev->get_stream_format(rs::stream::infrared) == rs::format::y16; - vpImage depth_display( - (unsigned int)rs.getIntrinsics(rs::stream::depth).height, - (unsigned int)rs.getIntrinsics(rs::stream::depth).width); - vpImage depth(depth_display.getHeight(), - depth_display.getWidth()); + vpImage depth_display((unsigned int)rs.getIntrinsics(rs::stream::depth).height, + (unsigned int)rs.getIntrinsics(rs::stream::depth).width); + vpImage depth(depth_display.getHeight(), depth_display.getWidth()); #ifdef VISP_HAVE_PCL - pcl::PointCloud::Ptr pointcloud( - new pcl::PointCloud); + pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud); #ifdef USE_THREAD - vpThread thread_display_pointcloud(displayPointcloudFunction, - (vpThread::Args)&pointcloud); + vpThread thread_display_pointcloud(displayPointcloudFunction, (vpThread::Args)&pointcloud); #else - pcl::visualization::PCLVisualizer::Ptr viewer( - new pcl::visualization::PCLVisualizer("3D Viewer")); - pcl::visualization::PointCloudColorHandlerRGBField rgb( - pointcloud); + pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); + pcl::visualization::PointCloudColorHandlerRGBField rgb(pointcloud); viewer->setBackgroundColor(0, 0, 0); viewer->addCoordinateSystem(1.0); viewer->initCameraParameters(); @@ -196,14 +163,11 @@ int main() #if defined(VISP_HAVE_X11) vpDisplayX dc(color, 10, 10, "Color image"); - vpDisplayX di(infrared_display, (int)color.getWidth() + 80, 10, - "Infrared image"); - vpDisplayX dd(depth_display, 10, (int)color.getHeight() + 80, - "Depth image"); + vpDisplayX di(infrared_display, (int)color.getWidth() + 80, 10, "Infrared image"); + vpDisplayX dd(depth_display, 10, (int)color.getHeight() + 80, "Depth image"); #elif defined(VISP_HAVE_GDI) vpDisplayGDI dc(color, 10, 10, "Color image"); - vpDisplayGDI di(infrared_display, color.getWidth() + 80, 10, - "Infrared image"); + vpDisplayGDI di(infrared_display, color.getWidth() + 80, 10, "Infrared image"); vpDisplayGDI dd(depth_display, 10, color.getHeight() + 80, "Depth image"); #else std::cout << "No image viewer is available..." << std::endl; @@ -217,12 +181,10 @@ int main() vpImageConvert::convert(infrared_y16, infrared_display); } else { #ifdef VISP_HAVE_PCL - rs.acquire((unsigned char *)color.bitmap, - (unsigned char *)depth.bitmap, NULL, pointcloud, + rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth.bitmap, NULL, pointcloud, (unsigned char *)infrared_display.bitmap); #else - rs.acquire((unsigned char *)color.bitmap, - (unsigned char *)depth.bitmap, NULL, + rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth.bitmap, NULL, (unsigned char *)infrared_display.bitmap); #endif } @@ -236,15 +198,12 @@ int main() #else static bool update = false; if (!update) { - viewer->addPointCloud(pointcloud, rgb, - "sample cloud"); - viewer->setPointCloudRenderingProperties( - pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); + viewer->addPointCloud(pointcloud, rgb, "sample cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); viewer->setPosition(color.getWidth() + 80, color.getHeight() + 80); update = true; } else { - viewer->updatePointCloud(pointcloud, rgb, - "sample cloud"); + viewer->updatePointCloud(pointcloud, rgb, "sample cloud"); } viewer->spinOnce(10); @@ -258,8 +217,7 @@ int main() vpDisplay::display(depth_display); vpDisplay::displayText(color, 15, 15, "Click to quit", vpColor::red); - if (vpDisplay::getClick(color, false) || - vpDisplay::getClick(infrared_display, false) || + if (vpDisplay::getClick(color, false) || vpDisplay::getClick(infrared_display, false) || vpDisplay::getClick(depth_display, false)) { break; } @@ -285,8 +243,8 @@ int main() } catch (const vpException &e) { std::cerr << "RealSense error " << e.what() << std::endl; } catch (const rs::error &e) { - std::cerr << "RealSense error calling " << e.get_failed_function() << "(" - << e.get_failed_args() << "): " << e.what() << std::endl; + std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() + << "): " << e.what() << std::endl; } catch (const std::exception &e) { std::cerr << e.what() << std::endl; } diff --git a/example/device/framegrabber/grabRealSense2.cpp b/example/device/framegrabber/grabRealSense2.cpp index 2ea3719ebc..78ee471d54 100644 --- a/example/device/framegrabber/grabRealSense2.cpp +++ b/example/device/framegrabber/grabRealSense2.cpp @@ -46,8 +46,7 @@ #include #include -#if defined(VISP_HAVE_REALSENSE2) && \ - defined(VISP_HAVE_CPP11_COMPATIBILITY) && \ +#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && \ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) #ifdef VISP_HAVE_PCL @@ -58,31 +57,22 @@ namespace { // Global variables -pcl::PointCloud::Ptr - pointcloud(new pcl::PointCloud()); -pcl::PointCloud::Ptr - pointcloud_color(new pcl::PointCloud()); +pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud()); +pcl::PointCloud::Ptr pointcloud_color(new pcl::PointCloud()); bool cancelled = false, update_pointcloud = false; class ViewerWorker { public: - explicit ViewerWorker(const bool color_mode, std::mutex &mutex) - : m_colorMode(color_mode), m_mutex(mutex) - { - } + explicit ViewerWorker(const bool color_mode, std::mutex &mutex) : m_colorMode(color_mode), m_mutex(mutex) {} void run() { std::string date = vpTime::getDateTime(); - pcl::visualization::PCLVisualizer::Ptr viewer( - new pcl::visualization::PCLVisualizer("3D Viewer " + date)); - pcl::visualization::PointCloudColorHandlerRGBField rgb( - pointcloud_color); - pcl::PointCloud::Ptr local_pointcloud( - new pcl::PointCloud()); - pcl::PointCloud::Ptr local_pointcloud_color( - new pcl::PointCloud()); + pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer " + date)); + pcl::visualization::PointCloudColorHandlerRGBField rgb(pointcloud_color); + pcl::PointCloud::Ptr local_pointcloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr local_pointcloud_color(new pcl::PointCloud()); viewer->setBackgroundColor(0, 0, 0); viewer->initCameraParameters(); @@ -116,26 +106,19 @@ class ViewerWorker if (init) { if (m_colorMode) { - viewer->addPointCloud(local_pointcloud_color, - rgb, "RGB sample cloud"); - viewer->setPointCloudRenderingProperties( - pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, - "RGB sample cloud"); + viewer->addPointCloud(local_pointcloud_color, rgb, "RGB sample cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, + "RGB sample cloud"); } else { - viewer->addPointCloud(local_pointcloud, - "sample cloud"); - viewer->setPointCloudRenderingProperties( - pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, - "sample cloud"); + viewer->addPointCloud(local_pointcloud, "sample cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); } init = false; } else { if (m_colorMode) { - viewer->updatePointCloud( - local_pointcloud_color, rgb, "RGB sample cloud"); + viewer->updatePointCloud(local_pointcloud_color, rgb, "RGB sample cloud"); } else { - viewer->updatePointCloud(local_pointcloud, - "sample cloud"); + viewer->updatePointCloud(local_pointcloud, "sample cloud"); } } } @@ -163,38 +146,22 @@ int main() config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30); rs.open(config); - std::cout << rs.getCameraParameters( - RS2_STREAM_COLOR, - vpCameraParameters::perspectiveProjWithoutDistortion) - << std::endl; - std::cout << rs.getCameraParameters( - RS2_STREAM_COLOR, - vpCameraParameters::perspectiveProjWithDistortion) - << std::endl; - std::cout << "Extrinsics cMd: \n" - << rs.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) + std::cout << rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithoutDistortion) << std::endl; - std::cout << "Extrinsics dMc: \n" - << rs.getTransformation(RS2_STREAM_DEPTH, RS2_STREAM_COLOR) + std::cout << rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithDistortion) << std::endl; - std::cout << "Extrinsics cMi: \n" - << rs.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_INFRARED) - << std::endl; - std::cout << "Extrinsics dMi: \n" - << rs.getTransformation(RS2_STREAM_DEPTH, RS2_STREAM_INFRARED) - << std::endl; - - vpImage color( - (unsigned int)rs.getIntrinsics(RS2_STREAM_COLOR).height, - (unsigned int)rs.getIntrinsics(RS2_STREAM_COLOR).width); - vpImage infrared( - (unsigned int)rs.getIntrinsics(RS2_STREAM_INFRARED).height, - (unsigned int)rs.getIntrinsics(RS2_STREAM_INFRARED).width); - vpImage depth_display( - (unsigned int)rs.getIntrinsics(RS2_STREAM_DEPTH).height, - (unsigned int)rs.getIntrinsics(RS2_STREAM_DEPTH).width); - vpImage depth(depth_display.getHeight(), - depth_display.getWidth()); + std::cout << "Extrinsics cMd: \n" << rs.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) << std::endl; + std::cout << "Extrinsics dMc: \n" << rs.getTransformation(RS2_STREAM_DEPTH, RS2_STREAM_COLOR) << std::endl; + std::cout << "Extrinsics cMi: \n" << rs.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_INFRARED) << std::endl; + std::cout << "Extrinsics dMi: \n" << rs.getTransformation(RS2_STREAM_DEPTH, RS2_STREAM_INFRARED) << std::endl; + + vpImage color((unsigned int)rs.getIntrinsics(RS2_STREAM_COLOR).height, + (unsigned int)rs.getIntrinsics(RS2_STREAM_COLOR).width); + vpImage infrared((unsigned int)rs.getIntrinsics(RS2_STREAM_INFRARED).height, + (unsigned int)rs.getIntrinsics(RS2_STREAM_INFRARED).width); + vpImage depth_display((unsigned int)rs.getIntrinsics(RS2_STREAM_DEPTH).height, + (unsigned int)rs.getIntrinsics(RS2_STREAM_DEPTH).width); + vpImage depth(depth_display.getHeight(), depth_display.getWidth()); #ifdef VISP_HAVE_PCL std::mutex mutex; @@ -205,8 +172,7 @@ int main() #if defined(VISP_HAVE_X11) vpDisplayX dc(color, 10, 10, "Color image"); vpDisplayX di(infrared, (int)color.getWidth() + 80, 10, "Infrared image"); - vpDisplayX dd(depth_display, 10, (int)color.getHeight() + 80, - "Depth image"); + vpDisplayX dd(depth_display, 10, (int)color.getHeight() + 80, "Depth image"); #elif defined(VISP_HAVE_GDI) vpDisplayGDI dc(color, 10, 10, "Color image"); vpDisplayGDI di(infrared, color.getWidth() + 80, 10, "Infrared image"); @@ -219,14 +185,12 @@ int main() #ifdef VISP_HAVE_PCL { std::lock_guard lock(mutex); - rs.acquire((unsigned char *)color.bitmap, - (unsigned char *)depth.bitmap, NULL, pointcloud_color, + rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth.bitmap, NULL, pointcloud_color, (unsigned char *)infrared.bitmap); update_pointcloud = true; } #else - rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth.bitmap, - NULL, (unsigned char *)infrared.bitmap); + rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth.bitmap, NULL, (unsigned char *)infrared.bitmap); #endif vpImageConvert::createDepthHistogram(depth, depth_display); @@ -236,8 +200,7 @@ int main() vpDisplay::display(depth_display); vpDisplay::displayText(color, 15, 15, "Click to quit", vpColor::red); - if (vpDisplay::getClick(color, false) || - vpDisplay::getClick(infrared, false) || + if (vpDisplay::getClick(color, false) || vpDisplay::getClick(infrared, false) || vpDisplay::getClick(depth_display, false)) { break; } @@ -274,8 +237,7 @@ int main() std::cout << "Install librealsense2." << std::endl; #endif #if !defined(VISP_HAVE_CPP11_COMPATIBILITY) - std::cout << "Build ViSP with C++11 compiler flag (cmake -DUSE_CPP11=ON)." - << std::endl; + std::cout << "Build ViSP with C++11 compiler flag (cmake -DUSE_CPP11=ON)." << std::endl; #endif return 0; } diff --git a/example/device/framegrabber/grabV4l2.cpp b/example/device/framegrabber/grabV4l2.cpp index 215ba1ea68..e48eda31cf 100644 --- a/example/device/framegrabber/grabV4l2.cpp +++ b/example/device/framegrabber/grabV4l2.cpp @@ -84,10 +84,9 @@ typedef enum { \param opath : Image filename when saving. */ -void usage(const char *name, const char *badparam, unsigned fps, - unsigned input, unsigned scale, long niter, char *device, - vpV4l2Grabber::vpV4l2PixelFormatType pixelformat, - const vpImage_type &image_type, const std::string &opath) +void usage(const char *name, const char *badparam, unsigned fps, unsigned input, unsigned scale, long niter, + char *device, vpV4l2Grabber::vpV4l2PixelFormatType pixelformat, const vpImage_type &image_type, + const std::string &opath) { fprintf(stdout, "\n\ Grab grey level images using the Video For Linux Two framegrabber. \n\ @@ -145,8 +144,7 @@ OPTIONS: Default\n\ by the extension of the file (ex .png, .pgm, ...) \n\ \n\ -h \n\ - Print the help.\n\n", device, fps, input, pixelformat, - vpV4l2Grabber::V4L2_MAX_FORMAT - 1, image_type, scale, niter, + Print the help.\n\n", device, fps, input, pixelformat, vpV4l2Grabber::V4L2_MAX_FORMAT - 1, image_type, scale, niter, opath.c_str()); if (badparam) @@ -174,10 +172,8 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, unsigned &fps, unsigned &input, - unsigned &scale, bool &display, bool &verbose, long &niter, - char *device, - vpV4l2Grabber::vpV4l2PixelFormatType &pixelformat, +bool getOptions(int argc, const char **argv, unsigned &fps, unsigned &input, unsigned &scale, bool &display, + bool &verbose, long &niter, char *device, vpV4l2Grabber::vpV4l2PixelFormatType &pixelformat, vpImage_type &image_type, bool &save, std::string &opath) { const char *optarg_; @@ -217,14 +213,12 @@ bool getOptions(int argc, const char **argv, unsigned &fps, unsigned &input, verbose = true; break; case 'h': - usage(argv[0], NULL, fps, input, scale, niter, device, pixelformat, - image_type, opath); + usage(argv[0], NULL, fps, input, scale, niter, device, pixelformat, image_type, opath); return false; break; default: - usage(argv[0], optarg_, fps, input, scale, niter, device, pixelformat, - image_type, opath); + usage(argv[0], optarg_, fps, input, scale, niter, device, pixelformat, image_type, opath); return false; break; } @@ -232,8 +226,7 @@ bool getOptions(int argc, const char **argv, unsigned &fps, unsigned &input, if ((c == 1) || (c == -1)) { // standalone param or error - usage(argv[0], NULL, fps, input, scale, niter, device, pixelformat, - image_type, opath); + usage(argv[0], NULL, fps, input, scale, niter, device, pixelformat, image_type, opath); std::cerr << "ERROR: " << std::endl; std::cerr << " Bad argument " << optarg_ << std::endl << std::endl; return false; @@ -257,8 +250,7 @@ int main(int argc, const char **argv) unsigned int opt_fps = 25; unsigned int opt_input = 0; unsigned int opt_scale = 1; - vpV4l2Grabber::vpV4l2PixelFormatType opt_pixelformat = - vpV4l2Grabber::V4L2_YUYV_FORMAT; + vpV4l2Grabber::vpV4l2PixelFormatType opt_pixelformat = vpV4l2Grabber::V4L2_YUYV_FORMAT; long opt_iter = 100; bool opt_verbose = false; bool opt_display = true; @@ -271,9 +263,8 @@ int main(int argc, const char **argv) vpImage_type opt_image_type = color_image; // Read the command line options - if (getOptions(argc, argv, opt_fps, opt_input, opt_scale, opt_display, - opt_verbose, opt_iter, opt_device, opt_pixelformat, - opt_image_type, opt_save, opt_opath) == false) { + if (getOptions(argc, argv, opt_fps, opt_input, opt_scale, opt_display, opt_verbose, opt_iter, opt_device, + opt_pixelformat, opt_image_type, opt_save, opt_opath) == false) { exit(-1); } @@ -302,15 +293,13 @@ int main(int argc, const char **argv) g.open(Ig); // Acquire an image g.acquire(Ig); - std::cout << "Grey image size: width : " << Ig.getWidth() - << " height: " << Ig.getHeight() << std::endl; + std::cout << "Grey image size: width : " << Ig.getWidth() << " height: " << Ig.getHeight() << std::endl; } else { // Open the framegrabber with the specified settings on color images g.open(Ic); // Acquire an image g.acquire(Ic); - std::cout << "Color image size: width : " << Ic.getWidth() - << " height: " << Ic.getHeight() << std::endl; + std::cout << "Color image size: width : " << Ic.getWidth() << " height: " << Ic.getHeight() << std::endl; } // We open a window using either X11 or GTK. @@ -374,8 +363,7 @@ int main(int argc, const char **argv) } // Print the iteration duration - std::cout << "time: " << vpTime::measureTimeMs() - t << " (ms)" - << std::endl; + std::cout << "time: " << vpTime::measureTimeMs() - t << " (ms)" << std::endl; } g.close(); @@ -389,8 +377,5 @@ int main(int argc, const char **argv) int main() { vpTRACE("X11 or GTK display are not available"); } #endif #else -int main() -{ - vpTRACE("Video 4 Linux 2 frame grabber drivers are not available"); -} +int main() { vpTRACE("Video 4 Linux 2 frame grabber drivers are not available"); } #endif diff --git a/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp b/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp index 8157a9fa1b..b5added7d4 100644 --- a/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp +++ b/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp @@ -7,7 +7,7 @@ with C++11 threads. */ -#if defined(VISP_HAVE_CPP11_COMPATIBILITY) && defined(VISP_HAVE_V4L2) && \ +#if defined(VISP_HAVE_CPP11_COMPATIBILITY) && defined(VISP_HAVE_V4L2) && \ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK)) #include @@ -55,8 +55,7 @@ OPTIONS: \n\ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); } -bool getOptions(int argc, char **argv, unsigned int &deviceCount, - bool &saveVideo) +bool getOptions(int argc, char **argv, unsigned int &deviceCount, bool &saveVideo) { const char *optarg; const char **argv1 = (const char **)argv; @@ -103,8 +102,7 @@ class FrameQueue }; FrameQueue() - : m_cancelled(false), m_cond(), m_queueColor(), - m_maxQueueSize(std::numeric_limits::max()), m_mutex() + : m_cancelled(false), m_cond(), m_queueColor(), m_maxQueueSize(std::numeric_limits::max()), m_mutex() { } @@ -153,10 +151,7 @@ class FrameQueue return image; } - void setMaxQueueSize(const size_t max_queue_size) - { - m_maxQueueSize = max_queue_size; - } + void setMaxQueueSize(const size_t max_queue_size) { m_maxQueueSize = max_queue_size; } private: bool m_cancelled; @@ -170,8 +165,7 @@ class StorageWorker { public: - StorageWorker(FrameQueue &queue, const std::string &filename, - const unsigned int width, const unsigned int height) + StorageWorker(FrameQueue &queue, const std::string &filename, const unsigned int width, const unsigned int height) : m_queue(queue), m_filename(filename), m_width(width), m_height(height) { } @@ -220,11 +214,7 @@ class ShareImage struct cancelled { }; - ShareImage() - : m_cancelled(false), m_cond(), m_mutex(), m_pImgData(NULL), - m_totalSize(0) - { - } + ShareImage() : m_cancelled(false), m_cond(), m_mutex(), m_pImgData(NULL), m_totalSize(0) {} virtual ~ShareImage() { @@ -270,8 +260,7 @@ class ShareImage } // Set the image to display - void setImage(const unsigned char *const imageData, - const unsigned int totalSize) + void setImage(const unsigned char *const imageData, const unsigned int totalSize) { std::lock_guard lock(m_mutex); @@ -307,14 +296,12 @@ void capture(vpV4l2Grabber *const pGrabber, ShareImage &share_image) pGrabber->acquire(local_img); // Update share_image - share_image.setImage((unsigned char *)local_img.bitmap, - local_img.getSize() * 4); + share_image.setImage((unsigned char *)local_img.bitmap, local_img.getSize() * 4); } } -void display(const unsigned int width, const unsigned int height, - const int win_x, const int win_y, const unsigned int deviceId, - ShareImage &share_image, FrameQueue &queue, const bool save) +void display(const unsigned int width, const unsigned int height, const int win_x, const int win_y, + const unsigned int deviceId, ShareImage &share_image, FrameQueue &queue, const bool save) { vpImage local_img(height, width); @@ -332,21 +319,18 @@ void display(const unsigned int width, const unsigned int height, try { vpMouseButton::vpMouseButtonType button; - vpImage I_red(height, width), I_green(height, width), - I_blue(height, width), I_alpha(height, width); + vpImage I_red(height, width), I_green(height, width), I_blue(height, width), I_alpha(height, width); ; - vpImage I_red_gaussian(height, width), - I_green_gaussian(height, width), I_blue_gaussian(height, width); - vpImage I_red_gaussian_double, I_green_gaussian_double, - I_blue_gaussian_double; + vpImage I_red_gaussian(height, width), I_green_gaussian(height, width), + I_blue_gaussian(height, width); + vpImage I_red_gaussian_double, I_green_gaussian_double, I_blue_gaussian_double; bool exit = false, gaussian_blur = false; while (!exit) { double t = vpTime::measureTimeMs(); // Get image - share_image.getImage((unsigned char *)local_img.bitmap, - local_img.getSize() * 4); + share_image.getImage((unsigned char *)local_img.bitmap, local_img.getSize() * 4); // Apply gaussian blur to simulate a computation on the image if (gaussian_blur) { @@ -356,19 +340,15 @@ void display(const unsigned int width, const unsigned int height, vpImageConvert::convert(I_green, I_green_gaussian_double); vpImageConvert::convert(I_blue, I_blue_gaussian_double); - vpImageFilter::gaussianBlur(I_red_gaussian_double, - I_red_gaussian_double, 21); - vpImageFilter::gaussianBlur(I_green_gaussian_double, - I_green_gaussian_double, 21); - vpImageFilter::gaussianBlur(I_blue_gaussian_double, - I_blue_gaussian_double, 21); + vpImageFilter::gaussianBlur(I_red_gaussian_double, I_red_gaussian_double, 21); + vpImageFilter::gaussianBlur(I_green_gaussian_double, I_green_gaussian_double, 21); + vpImageFilter::gaussianBlur(I_blue_gaussian_double, I_blue_gaussian_double, 21); vpImageConvert::convert(I_red_gaussian_double, I_red_gaussian); vpImageConvert::convert(I_green_gaussian_double, I_green_gaussian); vpImageConvert::convert(I_blue_gaussian_double, I_blue_gaussian); - vpImageConvert::merge(&I_red_gaussian, &I_green_gaussian, - &I_blue_gaussian, NULL, local_img); + vpImageConvert::merge(&I_red_gaussian, &I_green_gaussian, &I_blue_gaussian, NULL, local_img); } t = vpTime::measureTimeMs() - t; @@ -378,9 +358,7 @@ void display(const unsigned int width, const unsigned int height, vpDisplay::display(local_img); vpDisplay::displayText(local_img, 20, 20, ss.str(), vpColor::red); - vpDisplay::displayText( - local_img, 40, 20, - "Left click to quit, right click for Gaussian blur.", vpColor::red); + vpDisplay::displayText(local_img, 40, 20, "Left click to quit, right click for Gaussian blur.", vpColor::red); vpDisplay::flush(local_img); @@ -451,16 +429,13 @@ int main(int argc, char *argv[]) std::string parent_directory = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); for (size_t deviceId = 0; deviceId < grabbers.size(); deviceId++) { // Start the capture thread for the current camera stream - capture_threads.emplace_back(capture, grabbers[deviceId], - std::ref(share_images[deviceId])); + capture_threads.emplace_back(capture, grabbers[deviceId], std::ref(share_images[deviceId])); int win_x = deviceId * offsetX, win_y = deviceId * offsetY; // Start the display thread for the current camera stream - display_threads.emplace_back(display, grabbers[deviceId]->getWidth(), - grabbers[deviceId]->getHeight(), win_x, - win_y, deviceId, - std::ref(share_images[deviceId]), - std::ref(save_queues[deviceId]), saveVideo); + display_threads.emplace_back(display, grabbers[deviceId]->getWidth(), grabbers[deviceId]->getHeight(), win_x, win_y, + deviceId, std::ref(share_images[deviceId]), std::ref(save_queues[deviceId]), + saveVideo); if (saveVideo) { std::stringstream ss; @@ -470,9 +445,8 @@ int main(int argc, char *argv[]) ss << "/%06d.png"; std::string filename = ss.str(); - storages.emplace_back( - std::ref(save_queues[deviceId]), std::cref(filename), - grabbers[deviceId]->getWidth(), grabbers[deviceId]->getHeight()); + storages.emplace_back(std::ref(save_queues[deviceId]), std::cref(filename), grabbers[deviceId]->getWidth(), + grabbers[deviceId]->getHeight()); } } @@ -499,8 +473,7 @@ int main(int argc, char *argv[]) } if (saveVideo) { - std::cout << "\nWaiting for finishing thread to write images..." - << std::endl; + std::cout << "\nWaiting for finishing thread to write images..." << std::endl; } // We're done reading, cancel all the queues diff --git a/example/device/kinect/kinectAcquisition.cpp b/example/device/kinect/kinectAcquisition.cpp index 8c3f041be2..bf03d70f5a 100644 --- a/example/device/kinect/kinectAcquisition.cpp +++ b/example/device/kinect/kinectAcquisition.cpp @@ -48,8 +48,7 @@ #include #ifdef VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI)) #include #include @@ -118,8 +117,7 @@ int main() // A click to stop acquisition std::cout << "Click in one image to stop acquisition" << std::endl; - while (!vpDisplay::getClick(Idmap, false) && - !vpDisplay::getClick(Irgb, false)) { + while (!vpDisplay::getClick(Idmap, false) && !vpDisplay::getClick(Irgb, false)) { kinect.getDepthMap(dmap); kinect.getDepthMap(dmap, Idmap); kinect.getRGB(Irgb); @@ -157,10 +155,6 @@ int main() #endif #else -int main() -{ - std::cout << "You should install libfreenect to run this example" - << std::endl; -} +int main() { std::cout << "You should install libfreenect to run this example" << std::endl; } #endif diff --git a/example/device/laserscanner/SickLDMRS-Acq.cpp b/example/device/laserscanner/SickLDMRS-Acq.cpp index d69c36e758..9ec84aafef 100644 --- a/example/device/laserscanner/SickLDMRS-Acq.cpp +++ b/example/device/laserscanner/SickLDMRS-Acq.cpp @@ -50,8 +50,7 @@ #include #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) int main() { @@ -70,9 +69,7 @@ int main() continue; iter++; - std::cout << "iter: " << iter - << " time: " << vpTime::measureTimeMs() - t1 << " ms" - << std::endl; + std::cout << "iter: " << iter << " time: " << vpTime::measureTimeMs() - t1 << " ms" << std::endl; } return 0; } catch (vpException &e) { @@ -86,8 +83,7 @@ int main() int main() { std::cout << "This example is only working on UNIX platforms \n" - << "since the Sick LD-MRS driver was not ported to Windows." - << std::endl; + << "since the Sick LD-MRS driver was not ported to Windows." << std::endl; return 0; } diff --git a/example/device/laserscanner/SickLDMRS-Process.cpp b/example/device/laserscanner/SickLDMRS-Process.cpp index fe98f7aadb..d1dcb19c58 100644 --- a/example/device/laserscanner/SickLDMRS-Process.cpp +++ b/example/device/laserscanner/SickLDMRS-Process.cpp @@ -72,10 +72,8 @@ #include #include -#if (!defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__)))) && \ - (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_GTK)) +#if (!defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))) && \ + (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) static int save = 0; static int layerToDisplay = 0xF; // 0xF = 1111 => all the layers are selected @@ -137,8 +135,7 @@ void *laser_display_and_save_loop(void *) unsigned int iter = 0; for (;;) { -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_GTK)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) vpDisplay::display(map); #endif @@ -157,8 +154,7 @@ void *laser_display_and_save_loop(void *) // Parse the four layers for (int layer = 0; layer < 4; layer++) { if (!((0x1 << layer) & layerToDisplay)) { - std::cout << "Layer " << layer + 1 << " is not displayed" - << std::endl; + std::cout << "Layer " << layer + 1 << " is not displayed" << std::endl; continue; } @@ -166,18 +162,13 @@ void *laser_display_and_save_loop(void *) if (save) { // Set the scan data filename to store the measures - sprintf(filename, "%s/scan%04u-layer%d.txt", output_path.c_str(), - iter, layer + 1); + sprintf(filename, "%s/scan%04u-layer%d.txt", output_path.c_str(), iter, layer + 1); fdscan.open(filename); // Write the file header fdscan << "# Scan layer [1 to 4] : " << layer + 1 << std::endl - << "# Start timestamp (s) : " - << laserscan[layer].getStartTimestamp() - time_offset - << std::endl - << "# End timestamp (s) : " - << laserscan[layer].getEndTimestamp() - time_offset - << std::endl + << "# Start timestamp (s) : " << laserscan[layer].getStartTimestamp() - time_offset << std::endl + << "# End timestamp (s) : " << laserscan[layer].getEndTimestamp() - time_offset << std::endl << "# Data : \"radial distance (m)\" \"horizontal angle " "(rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z " "(m)\"" @@ -192,11 +183,9 @@ void *laser_display_and_save_loop(void *) for (unsigned int i = 0; i < pointsLayer.size(); i++) { p = pointsLayer[i]; E.set_i(height - resolution * p.getRadialDist() * cos(p.getHAngle())); - E.set_j(width / 2. - - resolution * p.getRadialDist() * sin(p.getHAngle())); + E.set_j(width / 2. - resolution * p.getRadialDist() * sin(p.getHAngle())); // std::cout << "E: " << E << std::endl; -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_GTK)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) vpDisplay::displayLine(map, O, E, color[layer]); #endif if (save) { @@ -208,8 +197,7 @@ void *laser_display_and_save_loop(void *) fdscan.close(); } } -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_GTK)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) vpDisplay::flush(map); #endif iter++; @@ -245,8 +233,7 @@ void *laser_acq_loop(void *) #endif iter++; - std::cout << "laser acq time: " << vpTime::measureTimeMs() - t1 - << std::endl; + std::cout << "laser acq time: " << vpTime::measureTimeMs() - t1 << std::endl; } return NULL; @@ -257,8 +244,7 @@ void *camera_acq_and_display_loop(void *) #ifdef VISP_HAVE_DC1394 try { // Initialize the firewire framegrabber - vp1394TwoGrabber - g; // Create a grabber based on libdc1394-2.x third party lib + vp1394TwoGrabber g; // Create a grabber based on libdc1394-2.x third party lib // If no camera found return if (g.getNumCameras() == 0) @@ -268,9 +254,8 @@ void *camera_acq_and_display_loop(void *) // g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60); vpImage I; // Create a gray level image container - vpImage - Q; // Create a quarter size gray level image container - g.acquire(I); // Acquire an image + vpImage Q; // Create a quarter size gray level image container + g.acquire(I); // Acquire an image I.quarterSizeImage(Q); vpDisplay *display = NULL; @@ -297,20 +282,17 @@ void *camera_acq_and_display_loop(void *) uint64_t timestamp; uint32_t id; for (;;) { - dc1394video_frame_t *frame = - g.dequeue(I, timestamp, id); // Acquire an image + dc1394video_frame_t *frame = g.dequeue(I, timestamp, id); // Acquire an image I.quarterSizeImage(Q); double image_timestamp = timestamp / 1000000. - time_offset; - std::cout << "camera timestamp: " << image_timestamp << " s " - << std::endl; + std::cout << "camera timestamp: " << image_timestamp << " s " << std::endl; if (save) { // Set the image filename sprintf(filename, "%s/image%04u.png", output_path.c_str(), iter); vpImageIo::write(Q, filename); fdimage_ts << filename << " " << image_timestamp << std::endl; } -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_GTK)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)) vpDisplay::display(Q); vpDisplay::flush(Q); #endif @@ -338,34 +320,29 @@ int main(int argc, const char **argv) // Create a directory with name "username" vpIoTools::makeDirectory(output_path); } catch (...) { - std::cout << "Cannot create " << output_path << " directory" - << std::endl; + std::cout << "Cannot create " << output_path << " directory" << std::endl; return EXIT_FAILURE; } } // Parse the command line to set the variables vpParseArgv::vpArgvInfo argTable[] = { - {"-layer", vpParseArgv::ARGV_INT, (char *)NULL, - (char *)&layerToDisplay, + {"-layer", vpParseArgv::ARGV_INT, (char *)NULL, (char *)&layerToDisplay, "The layer to display:\n" "\t\t. 0x1 for layer 1.\n" "\t\t. 0x2 for layer 2.\n" "\t\t. 0x4 for layer 3.\n" "\t\t. 0x8 for layer 4.\n" "\t\tTo display all the layers you should set 0xF value."}, - {"-save", vpParseArgv::ARGV_INT, (char *)NULL, (char *)&save, - "Turn to 1 in order to save data."}, + {"-save", vpParseArgv::ARGV_INT, (char *)NULL, (char *)&save, "Turn to 1 in order to save data."}, {"-h", vpParseArgv::ARGV_HELP, (char *)NULL, (char *)NULL, "Display one or more measured layers form a Sick LD-MRS laser " "scanner."}, - {(char *)NULL, vpParseArgv::ARGV_END, (char *)NULL, (char *)NULL, - (char *)NULL}}; + {(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_LEFTOVERS | vpParseArgv::ARGV_NO_ABBREV | vpParseArgv::ARGV_NO_DEFAULTS)) { return (EXIT_FAILURE); } @@ -375,11 +352,9 @@ int main(int argc, const char **argv) pthread_t thread_camera_acq; pthread_t thread_laser_acq; pthread_t thread_laser_display; - pthread_create(&thread_camera_acq, NULL, &camera_acq_and_display_loop, - NULL); + pthread_create(&thread_camera_acq, NULL, &camera_acq_and_display_loop, NULL); pthread_create(&thread_laser_acq, NULL, &laser_acq_loop, NULL); - pthread_create(&thread_laser_display, NULL, &laser_display_and_save_loop, - NULL); + pthread_create(&thread_laser_display, NULL, &laser_display_and_save_loop, NULL); pthread_join(thread_camera_acq, 0); pthread_join(thread_laser_acq, 0); pthread_join(thread_laser_display, 0); @@ -397,8 +372,7 @@ int main(int argc, const char **argv) int main() { std::cout << "This example is only working on UNIX platforms \n" - << "since the Sick LD-MRS driver was not ported to Windows." - << std::endl; + << "since the Sick LD-MRS driver was not ported to Windows." << std::endl; return 0; } diff --git a/example/device/light/ringLight.cpp b/example/device/light/ringLight.cpp index f1adc0b5d8..5f8eba7bb5 100644 --- a/example/device/light/ringLight.cpp +++ b/example/device/light/ringLight.cpp @@ -123,8 +123,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &on, int &nsec, - double &nmsec) +bool getOptions(int argc, const char **argv, bool &on, int &nsec, double &nmsec) { const char *optarg; int c; diff --git a/example/direct-visual-servoing/photometricVisualServoing.cpp b/example/direct-visual-servoing/photometricVisualServoing.cpp index 8d07da667a..767d121028 100644 --- a/example/direct-visual-servoing/photometricVisualServoing.cpp +++ b/example/direct-visual-servoing/photometricVisualServoing.cpp @@ -71,10 +71,8 @@ // List of allowed command line options #define GETOPTARGS "cdi:n:h" -void usage(const char *name, const char *badparam, std::string ipath, - int niter); -bool getOptions(int argc, const char **argv, std::string &ipath, - bool &click_allowed, bool &display, int &niter); +void usage(const char *name, const char *badparam, std::string ipath, int niter); +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter); /*! @@ -86,8 +84,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, \param niter : Number of iterations. */ -void usage(const char *name, const char *badparam, std::string ipath, - int niter) +void usage(const char *name, const char *badparam, std::string ipath, int niter) { fprintf(stdout, "\n\ Tracking of Surf key-points.\n\ @@ -135,8 +132,7 @@ OPTIONS: Default\n\ \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, int &niter) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter) { const char *optarg_; int c; @@ -198,8 +194,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, - opt_niter) == false) { + if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_niter) == false) { return (-1); } @@ -213,8 +208,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -223,13 +217,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, opt_niter); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -292,8 +283,7 @@ int main(int argc, const char **argv) vpDisplayOpenCV d; #endif -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_GTK) || defined(VISP_HAVE_OPENCV) +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_OPENCV) if (opt_display) { d.init(I, 20, 10, "Photometric visual servoing : s"); vpDisplay::display(I); @@ -311,8 +301,7 @@ int 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)); + 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 @@ -347,8 +336,7 @@ int main(int argc, const char **argv) #endif #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) if (opt_display) { - d1.init(Idiff, 40 + (int)I.getWidth(), 10, - "photometric visual servoing : s-s* "); + d1.init(Idiff, 40 + (int)I.getWidth(), 10, "photometric visual servoing : s-s* "); vpDisplay::display(Idiff); vpDisplay::flush(Idiff); } @@ -425,8 +413,7 @@ int main(int argc, const char **argv) double normeError = 0; do { - std::cout << "--------------------------------------------" << iter++ - << std::endl; + std::cout << "--------------------------------------------" << iter++ << std::endl; // Acquire the new image sim.setCameraPosition(cMo); diff --git a/example/homography/homographyHLM2DObject.cpp b/example/homography/homographyHLM2DObject.cpp index e6e03ef610..e0fe47ed92 100644 --- a/example/homography/homographyHLM2DObject.cpp +++ b/example/homography/homographyHLM2DObject.cpp @@ -187,8 +187,7 @@ int main(int argc, const char **argv) vpHomography::HLM(xb, yb, xa, ya, true, aHb); aHb /= aHb[2][2]; - std::cout << "aHb computed using the Malis paralax algorithm: \n" - << aHb << std::endl; + std::cout << "aHb computed using the Malis paralax algorithm: \n" << aHb << std::endl; vpRotationMatrix aRb; vpTranslationVector aTb; @@ -226,12 +225,10 @@ int main(int argc, const char **argv) std::cout << "Point " << i << std::endl; vpPoint p; std::cout << "("; - std::cout << aP[i].get_x() / aP[i].get_w() << ", " - << aP[i].get_y() / aP[i].get_w(); + std::cout << aP[i].get_x() / aP[i].get_w() << ", " << aP[i].get_y() / aP[i].get_w(); std::cout << ") = ("; p = aHb * bP[i]; - std::cout << p.get_x() / p.get_w() << ", " << p.get_y() / p.get_w() - << ")" << std::endl; + std::cout << p.get_x() / p.get_w() << ", " << p.get_y() / p.get_w() << ")" << std::endl; } std::cout << "-------------------------------" << std::endl; @@ -241,8 +238,7 @@ int main(int argc, const char **argv) std::list laTb; std::list lnb; - vpHomography::computeDisplacement(aHb, bP[0].get_x(), bP[0].get_y(), laRb, - laTb, lnb); + vpHomography::computeDisplacement(aHb, bP[0].get_x(), bP[0].get_y(), laRb, laTb, lnb); std::list::const_iterator it_laRb = laRb.begin(); std::list::const_iterator it_laTb = laTb.begin(); diff --git a/example/homography/homographyHLM3DObject.cpp b/example/homography/homographyHLM3DObject.cpp index 77f30af56d..95d4f63136 100644 --- a/example/homography/homographyHLM3DObject.cpp +++ b/example/homography/homographyHLM3DObject.cpp @@ -168,8 +168,7 @@ int main(int argc, const char **argv) P[10].setWorldCoordinates(-2 * L, -0.5 * L, 2 * L); vpHomogeneousMatrix bMo(0, 0, 1, 0, 0, 0); - vpHomogeneousMatrix aMb(0.1, 0.1, 0.1, vpMath::rad(10), 0, - vpMath::rad(40)); + vpHomogeneousMatrix aMb(0.1, 0.1, 0.1, vpMath::rad(10), 0, vpMath::rad(40)); vpHomogeneousMatrix aMo = aMb * bMo; for (unsigned int i = 0; i < nbpt; i++) { P[i].project(aMo); @@ -192,8 +191,7 @@ int main(int argc, const char **argv) std::cout << "Compare with built homography H = R + t/d n " << std::endl; vpPlane bp(0, 0, 1, 1); vpHomography aHb_built(aMb, bp); - std::cout << "aHb built from the displacement: \n" - << aHb_built / aHb_built[2][2] << std::endl; + std::cout << "aHb built from the displacement: \n" << aHb_built / aHb_built[2][2] << std::endl; aHb_built.computeDisplacement(aRb, aTb, n); std::cout << "Rotation: aRb" << std::endl; @@ -210,8 +208,7 @@ int main(int argc, const char **argv) vpHomography::HLM(xb, yb, xa, ya, false, aHb); - std::cout << "aHb computed using the Malis paralax algorithm" - << std::endl; + std::cout << "aHb computed using the Malis paralax algorithm" << std::endl; aHb /= aHb[2][2]; std::cout << std::endl << aHb << std::endl; @@ -232,12 +229,10 @@ int main(int argc, const char **argv) std::cout << "Point " << i << std::endl; vpPoint p; std::cout << "("; - std::cout << aP[i].get_x() / aP[i].get_w() << ", " - << aP[i].get_y() / aP[i].get_w(); + std::cout << aP[i].get_x() / aP[i].get_w() << ", " << aP[i].get_y() / aP[i].get_w(); std::cout << ") = ("; p = aHb * bP[i]; - std::cout << p.get_x() / p.get_w() << ", " << p.get_y() / p.get_w() - << ")" << std::endl; + std::cout << p.get_x() / p.get_w() << ", " << p.get_y() / p.get_w() << ")" << std::endl; } return 0; } catch (vpException &e) { diff --git a/example/homography/homographyHartleyDLT2DObject.cpp b/example/homography/homographyHartleyDLT2DObject.cpp index c431bcad32..d5c5d6e5d2 100644 --- a/example/homography/homographyHartleyDLT2DObject.cpp +++ b/example/homography/homographyHartleyDLT2DObject.cpp @@ -225,12 +225,10 @@ int main(int argc, const char **argv) std::cout << "Point " << i << std::endl; vpPoint p; std::cout << "("; - std::cout << aP[i].get_x() / aP[i].get_w() << ", " - << aP[i].get_y() / aP[i].get_w(); + std::cout << aP[i].get_x() / aP[i].get_w() << ", " << aP[i].get_y() / aP[i].get_w(); std::cout << ") = ("; p = aHb * bP[i]; - std::cout << p.get_x() / p.get_w() << ", " << p.get_y() / p.get_w() - << ")" << std::endl; + std::cout << p.get_x() / p.get_w() << ", " << p.get_y() / p.get_w() << ")" << std::endl; } return 0; } catch (vpException &e) { diff --git a/example/homography/homographyRansac2DObject.cpp b/example/homography/homographyRansac2DObject.cpp index 546d4a510a..08744dcf12 100644 --- a/example/homography/homographyRansac2DObject.cpp +++ b/example/homography/homographyRansac2DObject.cpp @@ -173,8 +173,7 @@ int main(int argc, const char **argv) inliers_ground_truth[10] = true; vpHomogeneousMatrix bMo(0, 0, 1, 0, 0, 0); - vpHomogeneousMatrix aMb(0.1, 0.1, 0.1, vpMath::rad(10), 0, - vpMath::rad(40)); + vpHomogeneousMatrix aMb(0.1, 0.1, 0.1, vpMath::rad(10), 0, vpMath::rad(40)); vpHomogeneousMatrix aMo = aMb * bMo; for (unsigned int i = 0; i < nbpt; i++) { P[i].project(aMo); @@ -195,8 +194,7 @@ int main(int argc, const char **argv) std::cout << "Compare with built homography H = R + t/d n " << std::endl; vpPlane bp(0, 0, 1, 1); vpHomography aHb_built(aMb, bp); - std::cout << "aHb built from the displacement: \n" - << aHb_built / aHb_built[2][2] << std::endl; + std::cout << "aHb built from the displacement: \n" << aHb_built / aHb_built[2][2] << std::endl; aHb_built.computeDisplacement(aRb, aTb, n); std::cout << "Rotation aRb: " << std::endl; @@ -212,8 +210,7 @@ int main(int argc, const char **argv) double residual; // Suppose px=1000. Set the threshold to 2 pixels => 2/1000 // In the data we have 6 inliers. We request that at least 6 are retrieved - vpHomography::ransac(xb, yb, xa, ya, aHb, inliers, residual, 6, - 2. / 1000); + vpHomography::ransac(xb, yb, xa, ya, aHb, inliers, residual, 6, 2. / 1000); std::cout << "aHb estimated using ransac:\n" << aHb << std::endl; std::cout << "Inliers indexes (should be 0,1,2,3,8,10): "; diff --git a/example/image/imageDiskRW.cpp b/example/image/imageDiskRW.cpp index a03fd8219a..80a05b5617 100644 --- a/example/image/imageDiskRW.cpp +++ b/example/image/imageDiskRW.cpp @@ -75,8 +75,7 @@ Print the program options. \param user : Username. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string opath, std::string user) +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\ @@ -122,8 +121,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, const std::string &user) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, const std::string &user) { const char *optarg_; int c; @@ -170,15 +168,13 @@ int main(int argc, const char **argv) std::string filename; std::string username; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " imageDiskRW.cpp" << std::endl << std::endl; std::cout << " reading and writting of PPM image" << std::endl; std::cout << " read an image that does not exist" << std::endl; std::cout << " write in a directory that does no exist" << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH @@ -190,8 +186,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Set the default output path -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX opt_opath = "/tmp"; #elif defined(_WIN32) opt_opath = "C:\\temp"; @@ -234,8 +229,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -244,13 +238,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, opath, username); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -275,8 +266,7 @@ int main(int argc, const char **argv) // an exception is thrown // Try to load a non existing image try { - filename = - vpIoTools::createFilePath(ipath, "image-that-does-not-exist.ppm"); + filename = vpIoTools::createFilePath(ipath, "image-that-does-not-exist.ppm"); vpImageIo::read(I, filename); } catch (vpException &e) { std::cout << "Catch an exception: " << e << std::endl; @@ -285,15 +275,13 @@ int main(int argc, const char **argv) // same thing if you to write in a directory that does not exist // or where you are not allowd to write. try { - filename = vpIoTools::createFilePath( - dirname, "directory-that-does-not-exist/Klimt.ppm"); + filename = vpIoTools::createFilePath(dirname, "directory-that-does-not-exist/Klimt.ppm"); vpImageIo::write(I, filename); } catch (vpException &e) { std::cout << "Catch an exception: " << e << std::endl; } - std::cout << "----------------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------------" << std::endl; // Let's consider that the image is now a color image (32 bits RGBa) vpImage Irgba; @@ -310,8 +298,7 @@ int main(int argc, const char **argv) // test io error try { - filename = - vpIoTools::createFilePath(ipath, "image-that-does-not-exist.ppm"); + filename = vpIoTools::createFilePath(ipath, "image-that-does-not-exist.ppm"); vpImageIo::read(Irgba, filename); } catch (vpException &e) { std::cout << "Catch an exception: " << e << std::endl; @@ -319,8 +306,7 @@ int main(int argc, const char **argv) // test io error try { - filename = vpIoTools::createFilePath( - dirname, "directory-that-does-not-exist/Klimt.ppm"); + filename = vpIoTools::createFilePath(dirname, "directory-that-does-not-exist/Klimt.ppm"); vpImageIo::write(Irgba, filename); } diff --git a/example/key-point/fernClassifier.cpp b/example/key-point/fernClassifier.cpp index 3c11369cef..fc897bf8f7 100644 --- a/example/key-point/fernClassifier.cpp +++ b/example/key-point/fernClassifier.cpp @@ -51,10 +51,8 @@ #include #include -#if ((defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI)) && \ - (VISP_HAVE_OPENCV_VERSION >= 0x020000) && \ - (VISP_HAVE_OPENCV_VERSION < 0x030000)) +#if ((defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && \ + (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000)) #include #include @@ -74,9 +72,8 @@ #define GETOPTARGS "hlcdb:i:p" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &isLearning, - std::string &dataFile, bool &click_allowed, bool &display, - bool &displayPoints, std::string &ipath); +bool getOptions(int argc, const char **argv, bool &isLearning, std::string &dataFile, bool &click_allowed, + bool &display, bool &displayPoints, std::string &ipath); /*! @@ -145,9 +142,8 @@ OPTIONS: \n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &isLearning, - std::string &dataFile, bool &click_allowed, bool &display, - bool &displayPoints, std::string &ipath) +bool getOptions(int argc, const char **argv, bool &isLearning, std::string &dataFile, bool &click_allowed, + bool &display, bool &displayPoints, std::string &ipath) { const char *optarg_; int c; @@ -219,8 +215,8 @@ int main(int argc, const char **argv) } // Read the command line options - if (getOptions(argc, argv, isLearning, dataFile, opt_click_allowed, - opt_display, displayPoints, opt_ipath) == false) { + if (getOptions(argc, argv, isLearning, dataFile, opt_click_allowed, opt_display, displayPoints, opt_ipath) == + false) { exit(-1); } @@ -235,8 +231,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -245,13 +240,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -287,8 +279,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -334,8 +325,7 @@ int main(int argc, const char **argv) if (opt_display) { // Display the rectangle which defines the part of the image where the // reference points are computed. - vpDisplay::displayRectangle(Iref, corners[0], corners[1], - vpColor::green); + vpDisplay::displayRectangle(Iref, corners[0], corners[1], vpColor::green); vpDisplay::flush(Iref); } @@ -393,9 +383,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed) { std::cout << "Click on the current image to continue" << std::endl; - vpDisplay::displayText(I, vpImagePoint(15, 15), - "Click on the current image to continue", - vpColor::red); + vpDisplay::displayText(I, vpImagePoint(15, 15), "Click on the current image to continue", vpColor::red); vpDisplay::flush(I); vpDisplay::getClick(I); } @@ -430,9 +418,7 @@ int main(int argc, const char **argv) std::cout << "unknown error line " << __LINE__ << std::endl; return -1; } - std::cout << "matching " << nbpts - << " points : " << vpTime::measureTimeMs() - t0 << " ms" - << std::endl; + std::cout << "matching " << nbpts << " points : " << vpTime::measureTimeMs() - t0 << " ms" << std::endl; if (opt_display) { fern.display(Iref, I, 7); @@ -455,8 +441,7 @@ int main(int argc, const char **argv) #else int main() { -#if (!(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI))) +#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 OpenCV-2.0.0 or a more recent release..."); diff --git a/example/key-point/keyPointSurf.cpp b/example/key-point/keyPointSurf.cpp index d5fad119fb..5918738a68 100644 --- a/example/key-point/keyPointSurf.cpp +++ b/example/key-point/keyPointSurf.cpp @@ -54,10 +54,8 @@ #include #include #include -#if ((defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI)) && \ - defined(VISP_HAVE_OPENCV_NONFREE) && \ - (VISP_HAVE_OPENCV_VERSION < 0x030000)) +#if ((defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && \ + defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)) #include @@ -75,8 +73,7 @@ #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); +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display); /*! @@ -131,8 +128,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -191,8 +187,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == - false) { + if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -206,8 +201,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -216,13 +210,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -260,8 +251,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -304,8 +294,7 @@ int main(int argc, const char **argv) if (opt_display) { // Display the rectangle which defines the part of the image where the // reference points are computed. - vpDisplay::displayRectangle(Iref, corners[0], corners[1], - vpColor::green); + vpDisplay::displayRectangle(Iref, corners[0], corners[1], vpColor::green); vpDisplay::flush(Iref); } @@ -328,8 +317,7 @@ int main(int argc, const char **argv) if (opt_display) { try { // Display size is automatically defined by the image (I) size - display[1].init(Icur, (int)(100 + Iref.getWidth()), 100, - "Display current image"); + display[1].init(Icur, (int)(100 + Iref.getWidth()), 100, "Display current image"); vpDisplay::display(Icur); vpDisplay::flush(Icur); } catch (...) { @@ -339,9 +327,7 @@ int main(int argc, const char **argv) } for (iter = 1; iter < 30; iter++) { - std::cout - << "----------------------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------------------" << std::endl; // set the new image name s.str(""); s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm"; @@ -360,8 +346,7 @@ int main(int argc, const char **argv) if (opt_display) { // Display the matched features surf.display(Iref, Icur, 7); - vpDisplay::displayRectangle(Iref, corners[0], corners[1], - vpColor::red); + vpDisplay::displayRectangle(Iref, corners[0], corners[1], vpColor::red); vpDisplay::flush(Iref); vpDisplay::flush(Icur); } @@ -376,8 +361,7 @@ int main(int argc, const char **argv) #else int main() { -#if (!(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI))) +#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 < 3.0.0 that contains " diff --git a/example/key-point/planarObjectDetector.cpp b/example/key-point/planarObjectDetector.cpp index b5234b032c..8bc60393dc 100644 --- a/example/key-point/planarObjectDetector.cpp +++ b/example/key-point/planarObjectDetector.cpp @@ -50,10 +50,8 @@ #include #include -#if ((defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI)) && \ - (VISP_HAVE_OPENCV_VERSION >= 0x020000) && \ - (VISP_HAVE_OPENCV_VERSION < 0x030000)) +#if ((defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && \ + (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000)) #include #include @@ -75,9 +73,8 @@ #define GETOPTARGS "hlcdb:i:p" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &isLearning, - std::string &dataFile, bool &click_allowed, bool &display, - bool &displayPoints, std::string &ipath); +bool getOptions(int argc, const char **argv, bool &isLearning, std::string &dataFile, bool &click_allowed, + bool &display, bool &displayPoints, std::string &ipath); /*! @@ -149,9 +146,8 @@ OPTIONS: \n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &isLearning, - std::string &dataFile, bool &click_allowed, bool &display, - bool &displayPoints, std::string &ipath) +bool getOptions(int argc, const char **argv, bool &isLearning, std::string &dataFile, bool &click_allowed, + bool &display, bool &displayPoints, std::string &ipath) { const char *optarg_; int c; @@ -223,8 +219,8 @@ int main(int argc, const char **argv) } // Read the command line options - if (getOptions(argc, argv, isLearning, dataFile, opt_click_allowed, - opt_display, displayPoints, opt_ipath) == false) { + if (getOptions(argc, argv, isLearning, dataFile, opt_click_allowed, opt_display, displayPoints, opt_ipath) == + false) { exit(-1); } @@ -239,8 +235,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -249,13 +244,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -291,8 +283,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -331,16 +322,14 @@ int main(int argc, const char **argv) std::cout << corners[i] << std::endl; } } else { - corners[0].set_ij(50, I.getWidth() - - 100); // small ROI for the automated test + corners[0].set_ij(50, I.getWidth() - 100); // small ROI for the automated test corners[1].set_ij(I.getHeight() - 100, I.getWidth() - 2); } if (opt_display) { // Display the rectangle which defines the part of the image where the // reference points are computed. - vpDisplay::displayRectangle(Iref, corners[0], corners[1], - vpColor::green); + vpDisplay::displayRectangle(Iref, corners[0], corners[1], vpColor::green); vpDisplay::flush(Iref); } @@ -360,12 +349,10 @@ int main(int argc, const char **argv) } double t0 = vpTime::measureTimeMs(); planar.buildReference(Iref, roi); - std::cout << "build reference in " << vpTime::measureTimeMs() - t0 - << " ms" << std::endl; + std::cout << "build reference in " << vpTime::measureTimeMs() - t0 << " ms" << std::endl; t0 = vpTime::measureTimeMs(); planar.recordDetector(objectName, dataFile); - std::cout << "record detector in " << vpTime::measureTimeMs() - t0 - << " ms" << std::endl; + std::cout << "record detector in " << vpTime::measureTimeMs() - t0 << " ms" << std::endl; } else { if (!vpIoTools::checkFilename(dataFile)) { vpERROR_TRACE("cannot load the database with the specified name. Has " @@ -390,9 +377,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed) { std::cout << "Click on the reference image to continue" << std::endl; - vpDisplay::displayText(Iref, vpImagePoint(15, 15), - "Click on the reference image to continue", - vpColor::red); + vpDisplay::displayText(Iref, vpImagePoint(15, 15), "Click on the reference image to continue", vpColor::red); vpDisplay::flush(Iref); vpDisplay::getClick(Iref); } @@ -416,8 +401,7 @@ int main(int argc, const char **argv) double t0 = vpTime::measureTimeMs(); // detection of the reference planar surface bool isDetected = planar.matchPoint(I); - std::cout << "matching in " << vpTime::measureTimeMs() - t0 << " ms" - << std::endl; + std::cout << "matching in " << vpTime::measureTimeMs() - t0 << " ms" << std::endl; if (isDetected) { vpHomography H; @@ -426,8 +410,7 @@ int main(int argc, const char **argv) if (opt_display) { if (isLearning) { vpDisplay::display(Iref); - vpDisplay::displayRectangle(Iref, corners[0], corners[1], - vpColor::green); + vpDisplay::displayRectangle(Iref, corners[0], corners[1], vpColor::green); planar.display(Iref, I, displayPoints); vpDisplay::flush(Iref); } else { @@ -455,8 +438,7 @@ int main(int argc, const char **argv) #else int main() { -#if (!(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI))) +#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 OpenCV-2.0.0 or a more recent release..."); diff --git a/example/manual/geometric-features/manGeometricFeatures.cpp b/example/manual/geometric-features/manGeometricFeatures.cpp index fa3b98e0ed..90d154d647 100644 --- a/example/manual/geometric-features/manGeometricFeatures.cpp +++ b/example/manual/geometric-features/manGeometricFeatures.cpp @@ -132,8 +132,7 @@ int main() // save the drawing vpImage Ic; vpDisplay::getImage(I, Ic); - std::cout << "ViSP creates \"./geometricFeatures.ppm\" B&W image " - << std::endl; + std::cout << "ViSP creates \"./geometricFeatures.ppm\" B&W image " << std::endl; vpImageIo::write(Ic, "./geometricFeatures.ppm"); return 0; } catch (vpException &e) { diff --git a/example/manual/hello-world/CMake/HelloWorld.cpp b/example/manual/hello-world/CMake/HelloWorld.cpp index 01623b83cb..3a148c3a24 100644 --- a/example/manual/hello-world/CMake/HelloWorld.cpp +++ b/example/manual/hello-world/CMake/HelloWorld.cpp @@ -48,9 +48,7 @@ int main() vpThetaUVector tu; // Construct a rotation matrix from the theta U angles - vpRotationMatrix R( - vpMath::rad(0.), - vpMath::rad(180) + 100 * std::numeric_limits::epsilon(), 0.); + vpRotationMatrix R(vpMath::rad(0.), vpMath::rad(180) + 100 * std::numeric_limits::epsilon(), 0.); // Extract the theta U angles from a rotation matrix tu.buildFrom(R); diff --git a/example/manual/image-manipulation/manDisplay.cpp b/example/manual/image-manipulation/manDisplay.cpp index 5c6b00a606..3dca565c69 100644 --- a/example/manual/image-manipulation/manDisplay.cpp +++ b/example/manual/image-manipulation/manDisplay.cpp @@ -121,8 +121,7 @@ int main() // Display in overlay a yellow string ip.set_i(300); ip.set_j(160); - vpDisplay::displayText(I, ip, "ViSP is a marvelous software", - vpColor::black); + vpDisplay::displayText(I, ip, "ViSP is a marvelous software", vpColor::black); // Flush the display : without this line nothing will appear on the screen vpDisplay::flush(I); diff --git a/example/manual/moments/manServoMomentsSimple.cpp b/example/manual/moments/manServoMomentsSimple.cpp index f5c4df69de..09594c7ebe 100644 --- a/example/manual/moments/manServoMomentsSimple.cpp +++ b/example/manual/moments/manServoMomentsSimple.cpp @@ -53,7 +53,7 @@ #include #include #include //init the feature database using the information about moment dependencies -#include //visual servoing task +#include //visual servoing task // this function converts the plane defined by the cMo to 1/Z=Ax+By+C plane // form @@ -87,10 +87,8 @@ int main() std::vector vec_p, vec_p_d; // vectors that contain the vertices of the contour polygon - 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 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 @@ -105,29 +103,24 @@ int main() vec_p_d.push_back(p); } - vpMomentObject cur( - 6); // Create a source moment object with 6 as maximum order + vpMomentObject cur(6); // Create a source moment object with 6 as maximum order cur.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a // countour polygon - cur.fromVector(vec_p); // Init the dense object with the source polygon + cur.fromVector(vec_p); // Init the dense object with the source polygon - vpMomentObject dst( - 6); // Create a destination moment object with 6 as maximum order + 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_d); // Init the dense object with the destination polygon + dst.fromVector(vec_p_d); // Init the dense object with the destination polygon // init classic moment primitives (for source) - vpMomentCommon mdb_cur( - vpMomentCommon::getSurface(dst), vpMomentCommon::getMu3(dst), - vpMomentCommon::getAlpha(dst)); // Init classic features + vpMomentCommon mdb_cur(vpMomentCommon::getSurface(dst), vpMomentCommon::getMu3(dst), + vpMomentCommon::getAlpha(dst)); // Init classic features vpFeatureMomentCommon fmdb_cur(mdb_cur); ////init classic moment primitives (for destination) - vpMomentCommon mdb_dst( - vpMomentCommon::getSurface(dst), vpMomentCommon::getMu3(dst), - vpMomentCommon::getAlpha(dst)); // Init classic features + vpMomentCommon mdb_dst(vpMomentCommon::getSurface(dst), vpMomentCommon::getMu3(dst), + vpMomentCommon::getAlpha(dst)); // Init classic features vpFeatureMomentCommon fmdb_dst(mdb_dst); // update+compute moment primitives from object (for destination) @@ -141,15 +134,12 @@ int main() task.setInteractionMatrixType(vpServo::CURRENT); task.setLambda(1); - task.addFeature(fmdb_cur.getFeatureGravityNormalized(), - fmdb_dst.getFeatureGravityNormalized()); + task.addFeature(fmdb_cur.getFeatureGravityNormalized(), fmdb_dst.getFeatureGravityNormalized()); task.addFeature(fmdb_cur.getFeatureAn(), fmdb_dst.getFeatureAn()); // the object is NOT symmetric // select C4 and C6 - task.addFeature(fmdb_cur.getFeatureCInvariant(), - fmdb_dst.getFeatureCInvariant(), - vpFeatureMomentCInvariant::selectC4() | - vpFeatureMomentCInvariant::selectC6()); + task.addFeature(fmdb_cur.getFeatureCInvariant(), fmdb_dst.getFeatureCInvariant(), + vpFeatureMomentCInvariant::selectC4() | vpFeatureMomentCInvariant::selectC6()); task.addFeature(fmdb_cur.getFeatureAlpha(), fmdb_dst.getFeatureAlpha()); vpBasicFeature *al = new vpFeatureMomentAlpha(mdb_dst, 0, 0, 1.); diff --git a/example/manual/ogre/HelloWorldOgre.cpp b/example/manual/ogre/HelloWorldOgre.cpp index d2cf03b785..e90a91bede 100644 --- a/example/manual/ogre/HelloWorldOgre.cpp +++ b/example/manual/ogre/HelloWorldOgre.cpp @@ -56,8 +56,7 @@ int main() { try { #if defined(VISP_HAVE_OGRE) -#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || \ - (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 @@ -126,8 +125,7 @@ int main() // - rotation of 180 deg along robot x axis to have head over feet // - rotation of -90 deg along y axis to have robot facing the camera ogre.setScale("Robot", 0.001f, 0.001f, 0.001f); - ogre.setRotation("Robot", - vpRotationMatrix(vpRxyzVector(M_PI, -M_PI / 2, 0))); + ogre.setRotation("Robot", vpRotationMatrix(vpRxyzVector(M_PI, -M_PI / 2, 0))); // Update projection matrix cMo[2][3] = 0.5; // Z = 0.5 meter @@ -150,8 +148,7 @@ int main() ogre.display(I, cMo); } #else - std::cout << "You need an available framegrabber to run this example" - << std::endl; + std::cout << "You need an available framegrabber to run this example" << std::endl; #endif #else std::cout << "You need Ogre3D to run this example" << std::endl; diff --git a/example/manual/ogre/HelloWorldOgreAdvanced.cpp b/example/manual/ogre/HelloWorldOgreAdvanced.cpp index 7f6f5fc76e..383963bb0a 100644 --- a/example/manual/ogre/HelloWorldOgreAdvanced.cpp +++ b/example/manual/ogre/HelloWorldOgreAdvanced.cpp @@ -63,8 +63,8 @@ class vpAROgreAdvanced : public vpAROgre Ogre::AnimationState *mAnimationState; public: - vpAROgreAdvanced(const vpCameraParameters &cam = vpCameraParameters(), - unsigned int width = 640, unsigned int height = 480) + vpAROgreAdvanced(const vpCameraParameters &cam = vpCameraParameters(), unsigned int width = 640, + unsigned int height = 480) : vpAROgre(cam, width, height) { mAnimationState = NULL; @@ -76,8 +76,7 @@ class vpAROgreAdvanced : public vpAROgre // Create the Entity Ogre::Entity *robot = mSceneMgr->createEntity("Robot", "robot.mesh"); // Attach robot to scene graph - Ogre::SceneNode *RobotNode = - mSceneMgr->getRootSceneNode()->createChildSceneNode("Robot"); + Ogre::SceneNode *RobotNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("Robot"); // RobotNode->setPosition((Ogre::Real)-0.3, (Ogre::Real)0.2, // (Ogre::Real)0); RobotNode->attachObject(robot); @@ -110,8 +109,7 @@ int main() { try { #if defined(VISP_HAVE_OGRE) -#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || \ - (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 @@ -179,8 +177,7 @@ int main() ogre.display(I, cMo); } #else - std::cout << "You need an available framegrabber to run this example" - << std::endl; + std::cout << "You need an available framegrabber to run this example" << std::endl; #endif #else std::cout << "You need Ogre3D to run this example" << std::endl; diff --git a/example/manual/simulation/manServo4PointsDisplay.cpp b/example/manual/simulation/manServo4PointsDisplay.cpp index d8da527195..4e7e6a44b8 100644 --- a/example/manual/simulation/manServo4PointsDisplay.cpp +++ b/example/manual/simulation/manServo4PointsDisplay.cpp @@ -50,8 +50,7 @@ #include #include -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) #include #include @@ -77,8 +76,7 @@ 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 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 diff --git a/example/manual/simulation/manSimu4Dots.cpp b/example/manual/simulation/manSimu4Dots.cpp index ff8c6af821..8f52c5f2f7 100644 --- a/example/manual/simulation/manSimu4Dots.cpp +++ b/example/manual/simulation/manSimu4Dots.cpp @@ -52,9 +52,7 @@ #include #include -#if (defined(VISP_HAVE_COIN3D_AND_GUI) && \ - (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || \ - defined(VISP_HAVE_GDI))) +#if (defined(VISP_HAVE_COIN3D_AND_GUI) && (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))) #include #include @@ -93,8 +91,7 @@ static void *mainLoop(void *_simu) /////////////////////////////////// // Set the initial camera location - vpHomogeneousMatrix cMo(0.3, 0.2, 3, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(40)); + 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 @@ -274,8 +271,7 @@ int main() if (!ipath.empty()) filename = vpIoTools::createFilePath(ipath, "iv/4points.iv"); - std::cout << "Load : " << filename << std::endl - << "This file should be in the working directory" << std::endl; + std::cout << "Load : " << filename << std::endl << "This file should be in the working directory" << std::endl; simu.load(filename.c_str()); diff --git a/example/manual/simulation/manSimu4Points.cpp b/example/manual/simulation/manSimu4Points.cpp index 4e98a6b6f0..5902091c4e 100644 --- a/example/manual/simulation/manSimu4Points.cpp +++ b/example/manual/simulation/manSimu4Points.cpp @@ -78,8 +78,7 @@ static void *mainLoop(void *_simu) ///////////////////////////////////////// // sets the initial camera location - vpHomogeneousMatrix cMo(-0.3, -0.2, 3, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(40)); + 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 @@ -222,8 +221,7 @@ int main() if (!ipath.empty()) filename = vpIoTools::createFilePath(ipath, "iv/4points.iv"); - std::cout << "Load : " << filename << std::endl - << "This file should be in the working directory" << std::endl; + std::cout << "Load : " << filename << std::endl << "This file should be in the working directory" << std::endl; simu.load(filename.c_str()); diff --git a/example/math/BSpline.cpp b/example/math/BSpline.cpp index 85def64bb4..31dab07023 100644 --- a/example/math/BSpline.cpp +++ b/example/math/BSpline.cpp @@ -68,16 +68,14 @@ #include #include -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || \ +#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); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -123,8 +121,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -243,42 +240,34 @@ int main(int argc, const char **argv) std::list knots_cur; bSpline.get_knots(knots_cur); unsigned int i_display = 0; - for (std::list::const_iterator it = knots_cur.begin(); - it != knots_cur.end(); ++it, ++i_display) { + for (std::list::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 controlPoints_cur; bSpline.get_controlPoints(controlPoints_cur); i_display = 0; - for (std::list::const_iterator - it = controlPoints_cur.begin(); - it != controlPoints_cur.end(); ++it, ++i_display) { + for (std::list::const_iterator it = controlPoints_cur.begin(); it != controlPoints_cur.end(); + ++it, ++i_display) { std::cout << i_display << " ---> " << *it << std::endl; } unsigned int i = bSpline.findSpan(5 / 2.0); - std::cout << "The knot interval number for the value u = 5/2 is : " << i - << std::endl; + std::cout << "The knot interval number for the value u = 5/2 is : " << i << std::endl; vpBasisFunction *N = NULL; N = bSpline.computeBasisFuns(5 / 2.0); - std::cout << "The nonvanishing basis functions N(u=5/2) are :" - << std::endl; + std::cout << "The nonvanishing basis functions N(u=5/2) are :" << std::endl; for (unsigned int j = 0; j < bSpline.get_p() + 1; j++) std::cout << N[j].value << std::endl; vpBasisFunction **N2 = NULL; N2 = bSpline.computeDersBasisFuns(5 / 2.0, 2); - std::cout - << "The first derivatives of the basis functions N'(u=5/2) are :" - << std::endl; + std::cout << "The first derivatives of the basis functions N'(u=5/2) are :" << std::endl; for (unsigned int j = 0; j < bSpline.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; + std::cout << "The second derivatives of the basis functions N''(u=5/2) are :" << std::endl; for (unsigned int j = 0; j < bSpline.get_p() + 1; j++) std::cout << N2[2][j].value << std::endl; @@ -289,8 +278,7 @@ int main(int argc, const char **argv) vpDisplay::displayCross(I, pt, 4, vpColor::red); u += 0.01; } - for (std::list::const_iterator it = controlPoints.begin(); - it != controlPoints.end(); ++it) { + for (std::list::const_iterator it = controlPoints.begin(); it != controlPoints.end(); ++it) { vpDisplay::displayCross(I, *it, 4, vpColor::green); } vpDisplay::flush(I); @@ -316,8 +304,7 @@ int main(int argc, const char **argv) int main() { std::cout << "This example requires a video device. " << std::endl - << "You should install X11, GTK, OpenCV, GDI or Direct3D" - << std::endl + << "You should install X11, GTK, OpenCV, GDI or Direct3D" << std::endl << "to be able to execute this example." << std::endl; return 0; } diff --git a/example/math/exponentialMap.cpp b/example/math/exponentialMap.cpp index b018857e0d..7154ec7b49 100644 --- a/example/math/exponentialMap.cpp +++ b/example/math/exponentialMap.cpp @@ -93,8 +93,7 @@ int main() M.extract(R); vpRxyzVector drxyz(R); // rotational displacement - std::cout << "Displacement if velocity is applied during 1 s : \n" - << dt << " " << drxyz << std::endl; + std::cout << "Displacement if velocity is applied during 1 s : \n" << dt << " " << drxyz << std::endl; } // Compute the displacement from the velocity applied during 2 seconds @@ -110,15 +109,13 @@ int main() M.extract(R); vpRxyzVector drxyz(R); // rotational displacement - std::cout << "Displacement if velocity is applied during 2 s : \n" - << dt << " " << drxyz << std::endl; + std::cout << "Displacement if velocity is applied during 2 s : \n" << dt << " " << drxyz << std::endl; } // Compute the velocity from the displacement observed during 2 seconds v = vpExponentialMap::inverse(M, 2.f); - std::cout << "Velocity from displacement observed during 2 s: \n" - << v << std::endl; + std::cout << "Velocity from displacement observed during 2 s: \n" << v << std::endl; return 0; } catch (vpException &e) { std::cout << "Catch an exception: " << e << std::endl; diff --git a/example/moments/image/servoMomentImage.cpp b/example/moments/image/servoMomentImage.cpp index 0e1b9715ce..61bb9e9e55 100644 --- a/example/moments/image/servoMomentImage.cpp +++ b/example/moments/image/servoMomentImage.cpp @@ -78,14 +78,11 @@ int main() std::cout << "You should install pthread third-party library." << std::endl; } // No display available -#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && \ - !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \ +#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \ !defined(VISP_HAVE_GTK) int main() { - std::cout - << "Can't run this example since no display capability is available." - << std::endl; + std::cout << "Can't run this example since no display capability is available." << std::endl; std::cout << "You should install one of the following third-party library: " "X11, OpenCV, GDI, GTK." << std::endl; @@ -115,11 +112,9 @@ int main() { try { // intial pose - vpHomogeneousMatrix cMo(-0.1, -0.1, 1.5, -vpMath::rad(20), - -vpMath::rad(20), -vpMath::rad(30)); + vpHomogeneousMatrix cMo(-0.1, -0.1, 1.5, -vpMath::rad(20), -vpMath::rad(20), -vpMath::rad(30)); // Desired pose - vpHomogeneousMatrix cdMo(vpHomogeneousMatrix( - 0.0, -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))); // init the simulation init(cMo, cdMo); @@ -152,11 +147,11 @@ vpHomogeneousMatrix cdMo; vpSimulatorCamera robot; // robot used in this simulation vpImage Iint(480, 640, 0); // internal image used for interface // display -vpServo task; // servoing task -vpCameraParameters cam; // robot camera parameters -double _error; // current error -vpImageSimulator imsim; // image simulator used to simulate the - // perspective-projection camera +vpServo task; // servoing task +vpCameraParameters cam; // robot camera parameters +double _error; // current error +vpImageSimulator imsim; // image simulator used to simulate the + // perspective-projection camera // several images used in the simulation vpImage cur_img(480, 640, 0); @@ -234,8 +229,7 @@ void init(vpHomogeneousMatrix &_cMo, vpHomogeneousMatrix &_cdMo) cMo = _cMo; // init source matrix cdMo = _cdMo; // init destination matrix - interaction_type = - vpServo::CURRENT; // use interaction matrix for current position + interaction_type = vpServo::CURRENT; // use interaction matrix for current position displayInt.init(Iint, 700, 0, "Visual servoing with moments"); @@ -275,12 +269,10 @@ void initFeatures() //////////////////////////////////// // don't need to be specific, vpMomentCommon automatically loads // Xg,Yg,An,Ci,Cj,Alpha moments - moments = new vpMomentCommon(vpMomentCommon ::getSurface(dst), - vpMomentCommon::getMu3(dst), + moments = new vpMomentCommon(vpMomentCommon ::getSurface(dst), vpMomentCommon::getMu3(dst), vpMomentCommon::getAlpha(dst), vec[2], true); - momentsDes = new vpMomentCommon( - vpMomentCommon::getSurface(dst), vpMomentCommon::getMu3(dst), - vpMomentCommon::getAlpha(dst), vec[2], true); + momentsDes = new vpMomentCommon(vpMomentCommon::getSurface(dst), vpMomentCommon::getMu3(dst), + vpMomentCommon::getAlpha(dst), vec[2], true); // same thing with common features featureMoments = new vpFeatureMomentCommon(*moments); featureMomentsDes = new vpFeatureMomentCommon(*momentsDes); @@ -294,17 +286,13 @@ void initFeatures() // 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()); + /// 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(), + task.addFeature(featureMoments->getFeatureCInvariant(), featureMomentsDes->getFeatureCInvariant(), (1 << 10) | (1 << 11)); - task.addFeature(featureMoments->getFeatureAlpha(), - featureMomentsDes->getFeatureAlpha()); + task.addFeature(featureMoments->getFeatureAlpha(), featureMomentsDes->getFeatureAlpha()); task.setLambda(1.); } @@ -397,8 +385,7 @@ void execute(unsigned int nbIter) vpMatrix tmpry, U; vpColVector singularvals; Linteraction.svd(singularvals, tmpry); - double condno = static_cast(singularvals.getMaxValue() / - singularvals.getMinValue()); + double condno = static_cast(singularvals.getMaxValue() / singularvals.getMinValue()); std::cout << "Condition Number: " << condno << std::endl; #endif } @@ -414,10 +401,7 @@ void execute(unsigned int nbIter) delete featureMomentsDes; } -void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) -{ - interaction_type = type; -} +void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { interaction_type = type; } double error() { return _error; } void planeToABC(vpPlane &pl, double &A, double &B, double &C) @@ -441,15 +425,13 @@ void init_visp_plot(vpPlot &ViSP_plot) * Initialize ViSP Plotting * ------------------------------------- */ - const unsigned int NbGraphs = 3; // No. of graphs - const unsigned int NbCurves_in_graph[NbGraphs] = { - 6, 6, 6}; // Curves in each graph + const unsigned int NbGraphs = 3; // No. of graphs + const unsigned int NbCurves_in_graph[NbGraphs] = {6, 6, 6}; // Curves in each graph ViSP_plot.init(NbGraphs, 800, 800, 10, 10, "Visual Servoing results..."); vpColor Colors[6] = {// Colour for s1, s2, s3, in 1st plot - vpColor::red, vpColor::green, vpColor::blue, - vpColor::orange, vpColor::cyan, vpColor::purple}; + vpColor::red, vpColor::green, vpColor::blue, vpColor::orange, vpColor::cyan, vpColor::purple}; for (unsigned int p = 0; p < NbGraphs; p++) { ViSP_plot.initGraph(p, NbCurves_in_graph[p]); diff --git a/example/moments/points/servoMomentPoints.cpp b/example/moments/points/servoMomentPoints.cpp index 977f982764..cb72b8fda7 100644 --- a/example/moments/points/servoMomentPoints.cpp +++ b/example/moments/points/servoMomentPoints.cpp @@ -72,14 +72,11 @@ int main() std::cout << "You should install pthread third-party library." << std::endl; } // No display available -#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && \ - !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \ +#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \ !defined(VISP_HAVE_GTK) int main() { - std::cout - << "Can't run this example since no display capability is available." - << std::endl; + std::cout << "Can't run this example since no display capability is available." << std::endl; std::cout << "You should install one of the following third-party library: " "X11, OpenCV, GDI, GTK." << std::endl; @@ -107,11 +104,9 @@ void removeJointLimits(vpSimulatorAfma6 &robot); int main() { try { // intial pose - vpHomogeneousMatrix cMo(0.05, 0.1, 1.5, vpMath::rad(30), vpMath::rad(20), - -vpMath::rad(15)); + vpHomogeneousMatrix cMo(0.05, 0.1, 1.5, vpMath::rad(30), vpMath::rad(20), -vpMath::rad(15)); // Desired pose - vpHomogeneousMatrix cdMo(vpHomogeneousMatrix( - 0.0, 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))); // init and run the simulation init(cMo, cdMo); @@ -142,13 +137,13 @@ vpHomogeneousMatrix cdMo; vpSimulatorAfma6 robot(false); // robot used in this simulation vpImage Iint(480, 640, - 255); // internal image used for interface display + 255); // internal image used for interface display vpServo::vpServoIteractionMatrixType interaction_type; // current or desired vpServo task; // servoing task -vpCameraParameters cam; // robot camera parameters -double _error; // current error -vpImageSimulator imsim; // image simulator used to simulate the - // perspective-projection camera +vpCameraParameters cam; // robot camera parameters +double _error; // current error +vpImageSimulator imsim; // image simulator used to simulate the + // perspective-projection camera // moment sets and their corresponding features vpMomentCommon *moments; @@ -215,11 +210,9 @@ void initFeatures() //////////////////////////////////// // don't need to be specific, vpMomentCommon automatically loads // Xg,Yg,An,Ci,Cj,Alpha moments - moments = new vpMomentCommon(vpMomentCommon ::getSurface(dst), - vpMomentCommon::getMu3(dst), + moments = new vpMomentCommon(vpMomentCommon ::getSurface(dst), vpMomentCommon::getMu3(dst), vpMomentCommon::getAlpha(dst), vec[2]); - momentsDes = new vpMomentCommon(vpMomentCommon::getSurface(dst), - vpMomentCommon::getMu3(dst), + momentsDes = new vpMomentCommon(vpMomentCommon::getSurface(dst), vpMomentCommon::getMu3(dst), vpMomentCommon::getAlpha(dst), vec[2]); // same thing with common features featureMoments = new vpFeatureMomentCommon(*moments); @@ -234,16 +227,12 @@ void initFeatures() // 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()); - task.addFeature(featureMoments->getFeatureCInvariant(), - featureMomentsDes->getFeatureCInvariant(), + /// task////////////////////////////// + task.addFeature(featureMoments->getFeatureGravityNormalized(), featureMomentsDes->getFeatureGravityNormalized()); + task.addFeature(featureMoments->getFeatureAn(), featureMomentsDes->getFeatureAn()); + task.addFeature(featureMoments->getFeatureCInvariant(), featureMomentsDes->getFeatureCInvariant(), (1 << 3) | (1 << 5)); - task.addFeature(featureMoments->getFeatureAlpha(), - featureMomentsDes->getFeatureAlpha()); + task.addFeature(featureMoments->getFeatureAlpha(), featureMomentsDes->getFeatureAlpha()); task.setLambda(1.); } @@ -268,10 +257,9 @@ void refreshScene(vpMomentObject &obj) void init(vpHomogeneousMatrix &_cMo, vpHomogeneousMatrix &_cdMo) { - cMo = _cMo; // init source matrix - cdMo = _cdMo; // init destination matrix - interaction_type = - vpServo::CURRENT; // use interaction matrix for current position + cMo = _cMo; // init source matrix + cdMo = _cdMo; // init destination matrix + interaction_type = vpServo::CURRENT; // use interaction matrix for current position displayInt.init(Iint, 700, 0, "Visual servoing with moments"); @@ -382,14 +370,12 @@ void planeToABC(vpPlane &pl, double &A, double &B, double &C) void paramRobot() { /*Initialise the robot and especially the camera*/ - robot.init(vpAfma6::TOOL_CCMOP, - vpCameraParameters::perspectiveProjWithoutDistortion); + robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion); robot.setCurrentViewColor(vpColor(150, 150, 150)); robot.setDesiredViewColor(vpColor(200, 200, 200)); robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); removeJointLimits(robot); - robot.initScene(vpWireFrameSimulator::POINT_CLOUD, - vpWireFrameSimulator::D_STANDARD); + robot.initScene(vpWireFrameSimulator::POINT_CLOUD, vpWireFrameSimulator::D_STANDARD); robot.setConstantSamplingTimeMode(true); /*Initialise the position of the object relative to the pose of the robot's * camera*/ @@ -400,10 +386,7 @@ void paramRobot() robot.getCameraParameters(cam, Iint); } -void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) -{ - interaction_type = type; -} +void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { interaction_type = type; } double error() { return _error; } #endif diff --git a/example/moments/polygon/servoMomentPolygon.cpp b/example/moments/polygon/servoMomentPolygon.cpp index efec28834f..0ffae75690 100644 --- a/example/moments/polygon/servoMomentPolygon.cpp +++ b/example/moments/polygon/servoMomentPolygon.cpp @@ -71,14 +71,11 @@ int main() std::cout << "You should install pthread third-party library." << std::endl; } // No display available -#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && \ - !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \ +#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \ !defined(VISP_HAVE_GTK) int main() { - std::cout - << "Can't run this example since no display capability is available." - << std::endl; + std::cout << "Can't run this example since no display capability is available." << std::endl; std::cout << "You should install one of the following third-party library: " "X11, OpenCV, GDI, GTK." << std::endl; @@ -106,11 +103,9 @@ void removeJointLimits(vpSimulatorAfma6 &robot); int main() { try { // intial pose - vpHomogeneousMatrix cMo(-0.1, -0.1, 1.5, -vpMath::rad(20), - -vpMath::rad(20), -vpMath::rad(30)); + vpHomogeneousMatrix cMo(-0.1, -0.1, 1.5, -vpMath::rad(20), -vpMath::rad(20), -vpMath::rad(30)); // Desired pose - vpHomogeneousMatrix cdMo(vpHomogeneousMatrix( - 0.0, 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))); // init and run the simulation init(cMo, cdMo); @@ -141,13 +136,13 @@ vpHomogeneousMatrix cdMo; vpSimulatorAfma6 robot(false); // robot used in this simulation vpImage Iint(480, 640, - 255); // internal image used for interface display -vpServo task; // servoing task -vpCameraParameters cam; // robot camera parameters -double _error; // current error + 255); // internal image used for interface display +vpServo task; // servoing task +vpCameraParameters cam; // robot camera parameters +double _error; // current error vpServo::vpServoIteractionMatrixType interaction_type; // current or desired -vpImageSimulator imsim; // image simulator used to simulate the - // perspective-projection camera +vpImageSimulator imsim; // image simulator used to simulate the + // perspective-projection camera // source and destination objects for moment manipulation vpMomentObject src(6); @@ -244,11 +239,9 @@ void initFeatures() //////////////////////////////////// // don't need to be specific, vpMomentCommon automatically loads // Xg,Yg,An,Ci,Cj,Alpha moments - moments = new vpMomentCommon(vpMomentCommon ::getSurface(dst), - vpMomentCommon::getMu3(dst), + moments = new vpMomentCommon(vpMomentCommon ::getSurface(dst), vpMomentCommon::getMu3(dst), vpMomentCommon::getAlpha(dst), vec[2]); - momentsDes = new vpMomentCommon(vpMomentCommon::getSurface(dst), - vpMomentCommon::getMu3(dst), + momentsDes = new vpMomentCommon(vpMomentCommon::getSurface(dst), vpMomentCommon::getMu3(dst), vpMomentCommon::getAlpha(dst), vec[2]); // same thing with common features featureMoments = new vpFeatureMomentCommon(*moments); @@ -263,17 +256,13 @@ void initFeatures() // 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()); + /// 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(), + task.addFeature(featureMoments->getFeatureCInvariant(), featureMomentsDes->getFeatureCInvariant(), (1 << 10) | (1 << 11)); - task.addFeature(featureMoments->getFeatureAlpha(), - featureMomentsDes->getFeatureAlpha()); + task.addFeature(featureMoments->getFeatureAlpha(), featureMomentsDes->getFeatureAlpha()); task.setLambda(0.4); } @@ -341,10 +330,7 @@ void execute(unsigned int nbIter) delete featureMomentsDes; } -void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) -{ - interaction_type = type; -} +void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { interaction_type = type; } double error() { return _error; } void removeJointLimits(vpSimulatorAfma6 &robot_) @@ -384,14 +370,12 @@ void planeToABC(vpPlane &pl, double &A, double &B, double &C) void paramRobot() { /*Initialise the robot and especially the camera*/ - robot.init(vpAfma6::TOOL_CCMOP, - vpCameraParameters::perspectiveProjWithoutDistortion); + robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion); robot.setCurrentViewColor(vpColor(150, 150, 150)); robot.setDesiredViewColor(vpColor(200, 200, 200)); robot.setRobotState(vpRobot::STATE_POSITION_CONTROL); removeJointLimits(robot); - robot.initScene(vpWireFrameSimulator::RECTANGLE, - vpWireFrameSimulator::D_STANDARD); + robot.initScene(vpWireFrameSimulator::RECTANGLE, vpWireFrameSimulator::D_STANDARD); /*Initialise the position of the object relative to the pose of the robot's * camera*/ robot.initialiseObjectRelativeToCamera(cMo); diff --git a/example/ogre-simulator/AROgre.cpp b/example/ogre-simulator/AROgre.cpp index 04e512f262..5fbd7f2838 100644 --- a/example/ogre-simulator/AROgre.cpp +++ b/example/ogre-simulator/AROgre.cpp @@ -46,11 +46,10 @@ #include //#if defined(VISP_HAVE_OGRE) && (defined(VISP_HAVE_OPENCV) || -//defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) +// defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) //|| (defined(VISP_HAVE_X11) && ! defined(APPLE))) -#if defined(VISP_HAVE_OGRE) && \ - (defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) || \ +#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__) && defined(__MACH__)))) //#if defined(VISP_HAVE_X11) && ! defined(APPLE) @@ -90,8 +89,7 @@ */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath) +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath) { fprintf(stdout, "\n\ Test augmented reality using the vpAROgre class.\n\ @@ -145,8 +143,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &ppath, bool &click_allowed) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, bool &click_allowed) { const char *optarg; int c; @@ -191,9 +188,8 @@ class vpAROgreExample : public vpAROgre { public: // The constructor doesn't change here - vpAROgreExample(const vpCameraParameters &mcam = vpCameraParameters(), - unsigned int width = 640, unsigned int height = 480, - const char *resourcePath = NULL) + vpAROgreExample(const vpCameraParameters &mcam = vpCameraParameters(), unsigned int width = 640, + unsigned int height = 480, const char *resourcePath = NULL) : vpAROgre(mcam, width, height) { // Direction vectors @@ -218,24 +214,21 @@ class vpAROgreExample : public vpAROgre void createScene() { // Lumieres - mSceneMgr->setAmbientLight(Ogre::ColourValue( - (float)0.6, (float)0.6, (float)0.6)); // Default value of lightning + mSceneMgr->setAmbientLight(Ogre::ColourValue((float)0.6, (float)0.6, (float)0.6)); // Default value of lightning Ogre::Light *light = mSceneMgr->createLight(); light->setDiffuseColour(1.0, 1.0, 1.0); // scaled RGB values light->setSpecularColour(1.0, 1.0, 1.0); // scaled RGB values // Lumiere ponctuelle light->setPosition(-5, -5, 10); light->setType(Ogre::Light::LT_POINT); - light->setAttenuation((Ogre::Real)100, (Ogre::Real)1.0, (Ogre::Real)0.045, - (Ogre::Real)0.0075); + light->setAttenuation((Ogre::Real)100, (Ogre::Real)1.0, (Ogre::Real)0.045, (Ogre::Real)0.0075); // Ombres light->setCastShadows(true); // Create the Entity robot = mSceneMgr->createEntity("Robot", "robot.mesh"); // Attach robot to scene graph - Ogre::SceneNode *RobotNode = - mSceneMgr->getRootSceneNode()->createChildSceneNode("Robot"); + Ogre::SceneNode *RobotNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("Robot"); RobotNode->attachObject(robot); RobotNode->scale((Ogre::Real)0.001, (Ogre::Real)0.001, (Ogre::Real)0.001); RobotNode->pitch(Ogre::Degree(90)); @@ -255,12 +248,10 @@ class vpAROgreExample : public vpAROgre Ogre::Plane plan; plan.d = 0; plan.normal = Ogre::Vector3::UNIT_Z; - Ogre::MeshManager::getSingleton().createPlane( - "sol", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, plan, - (Ogre::Real)0.22, (Ogre::Real)0.16, 10, 10, true, 1, 1, 1); + Ogre::MeshManager::getSingleton().createPlane("sol", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, plan, + (Ogre::Real)0.22, (Ogre::Real)0.16, 10, 10, true, 1, 1, 1); Ogre::Entity *ent = mSceneMgr->createEntity("Entitesol", "sol"); - Ogre::SceneNode *PlaneNode = - mSceneMgr->getRootSceneNode()->createChildSceneNode("Entitesol"); + Ogre::SceneNode *PlaneNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("Entitesol"); PlaneNode->attachObject(ent); ent->setMaterialName("Examples/GrassFloor"); } @@ -287,33 +278,25 @@ class vpAROgreExample : public vpAROgre bool event = false; // Check entries if (mKeyboard->isKeyDown(OIS::KC_Z) || mKeyboard->isKeyDown(OIS::KC_UP)) { - mSceneMgr->getSceneNode("Robot")->setPosition( - mSceneMgr->getSceneNode("Robot")->getPosition() + - (Ogre::Real)0.003 * vecDevant); + mSceneMgr->getSceneNode("Robot")->setPosition(mSceneMgr->getSceneNode("Robot")->getPosition() + + (Ogre::Real)0.003 * vecDevant); event = true; } - if (mKeyboard->isKeyDown(OIS::KC_S) || - mKeyboard->isKeyDown(OIS::KC_DOWN)) { - mSceneMgr->getSceneNode("Robot")->setPosition( - mSceneMgr->getSceneNode("Robot")->getPosition() - - (Ogre::Real)0.003 * vecDevant); + if (mKeyboard->isKeyDown(OIS::KC_S) || mKeyboard->isKeyDown(OIS::KC_DOWN)) { + mSceneMgr->getSceneNode("Robot")->setPosition(mSceneMgr->getSceneNode("Robot")->getPosition() - + (Ogre::Real)0.003 * vecDevant); event = true; } - if (mKeyboard->isKeyDown(OIS::KC_Q) || - mKeyboard->isKeyDown(OIS::KC_LEFT)) { - rotmy = Ogre::Matrix3((Ogre::Real)cos(-angle), (Ogre::Real)sin(-angle), - 0, (Ogre::Real)(-sin(-angle)), + if (mKeyboard->isKeyDown(OIS::KC_Q) || mKeyboard->isKeyDown(OIS::KC_LEFT)) { + rotmy = Ogre::Matrix3((Ogre::Real)cos(-angle), (Ogre::Real)sin(-angle), 0, (Ogre::Real)(-sin(-angle)), (Ogre::Real)cos(-angle), 0, 0, 0, 1); vecDevant = vecDevant * rotmy; - mSceneMgr->getSceneNode("Robot")->yaw( - Ogre::Radian((Ogre::Real)(-angle))); + mSceneMgr->getSceneNode("Robot")->yaw(Ogre::Radian((Ogre::Real)(-angle))); event = true; } - if (mKeyboard->isKeyDown(OIS::KC_D) || - mKeyboard->isKeyDown(OIS::KC_RIGHT)) { - rotmy = Ogre::Matrix3((Ogre::Real)cos(angle), (Ogre::Real)sin(angle), 0, - (Ogre::Real)(-sin(angle)), (Ogre::Real)cos(angle), - 0, 0, 0, 1); + if (mKeyboard->isKeyDown(OIS::KC_D) || mKeyboard->isKeyDown(OIS::KC_RIGHT)) { + rotmy = Ogre::Matrix3((Ogre::Real)cos(angle), (Ogre::Real)sin(angle), 0, (Ogre::Real)(-sin(angle)), + (Ogre::Real)cos(angle), 0, 0, 0, 1); vecDevant = vecDevant * rotmy; mSceneMgr->getSceneNode("Robot")->yaw(Ogre::Radian((Ogre::Real)angle)); event = true; @@ -341,10 +324,8 @@ class vpAROgreExample : public vpAROgre situated at (-7,6,0),(7,6,0),(7,-6,0),(-7,-6,0) (unit = cm) in your real world */ -void computeInitialPose(vpCameraParameters *mcam, vpImage &I, - vpPose *mPose, vpDot2 *md, vpImagePoint *mcog, - vpHomogeneousMatrix *cMo, vpPoint *mP, - const bool &opt_click_allowed) +void computeInitialPose(vpCameraParameters *mcam, vpImage &I, vpPose *mPose, vpDot2 *md, + vpImagePoint *mcog, vpHomogeneousMatrix *cMo, vpPoint *mP, const bool &opt_click_allowed) { // --------------------------------------------------- // Code inspired from ViSP example of camera pose @@ -563,8 +544,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed) == - false) { + if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed) == false) { exit(-1); } @@ -578,8 +558,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -588,16 +567,12 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) { usage(argv[0], NULL, ipath, opt_ppath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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 option if you want to " - << std::endl - << " use personal images." << std::endl - << std::endl; + std::cerr << " Use -i 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 option if you want to " << std::endl + << " use personal images." << std::endl + << std::endl; exit(-1); } @@ -651,11 +626,9 @@ int main(int argc, const char **argv) vpCTRACE << "Load: " << filename << std::endl; grabber.open(Idisplay); grabber.acquire(Idisplay); - vpCameraParameters mcamTmp(592, 570, grabber.getWidth() / 2, - grabber.getHeight() / 2); + vpCameraParameters mcamTmp(592, 570, grabber.getWidth() / 2, grabber.getHeight() / 2); // Compute the initial pose of the camera - computeInitialPose(&mcamTmp, Idisplay, &mPose, md, mcog, &cMo, mP, - opt_click_allowed); + computeInitialPose(&mcamTmp, Idisplay, &mPose, md, mcog, &cMo, mP, opt_click_allowed); // Close the framegrabber grabber.close(); @@ -670,14 +643,12 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } // Create a vpRAOgre object with color background - vpAROgreExample ogre(mcam, (unsigned int)grabber.getWidth(), - (unsigned int)grabber.getHeight()); + vpAROgreExample ogre(mcam, (unsigned int)grabber.getWidth(), (unsigned int)grabber.getHeight()); // Initialize it ogre.init(IC); @@ -733,8 +704,7 @@ 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; + std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl; return 1; } catch (...) { std::cout << "Catch an exception " << std::endl; diff --git a/example/ogre-simulator/AROgreBasic.cpp b/example/ogre-simulator/AROgreBasic.cpp index cc44cfe4de..b8774e03f3 100644 --- a/example/ogre-simulator/AROgreBasic.cpp +++ b/example/ogre-simulator/AROgreBasic.cpp @@ -46,11 +46,10 @@ #include //#if defined(VISP_HAVE_OGRE) && (defined(VISP_HAVE_OPENCV) || -//defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) +// defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) //|| (defined(VISP_HAVE_X11) && ! defined(APPLE))) -#if defined(VISP_HAVE_OGRE) && \ - (defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) || \ +#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__) && defined(__MACH__)))) //#if defined(VISP_HAVE_X11) && ! defined(APPLE) @@ -89,8 +88,7 @@ \param ppath : Personal image path. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath) +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath) { fprintf(stdout, "\n\ Test augmented reality using the vpAROgre class.\n\ @@ -144,8 +142,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &ppath, bool &click_allowed) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, bool &click_allowed) { const char *optarg; int c; @@ -190,10 +187,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, situated at (-7,6,0),(7,6,0),(7,-6,0),(-7,-6,0) (unit = cm) in your real world */ -void computeInitialPose(vpCameraParameters *mcam, vpImage &I, - vpPose *mPose, vpDot2 *md, vpImagePoint *mcog, - vpHomogeneousMatrix *cMo, vpPoint *mP, - const bool &opt_click_allowed) +void computeInitialPose(vpCameraParameters *mcam, vpImage &I, vpPose *mPose, vpDot2 *md, + vpImagePoint *mcog, vpHomogeneousMatrix *cMo, vpPoint *mP, const bool &opt_click_allowed) { // --------------------------------------------------- // Code inspired from ViSP example of camera pose @@ -414,8 +409,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed) == - false) { + if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed) == false) { exit(-1); } @@ -429,8 +423,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -439,16 +432,12 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) { usage(argv[0], NULL, ipath, opt_ppath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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 option if you want to " - << std::endl - << " use personal images." << std::endl - << std::endl; + std::cerr << " Use -i 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 option if you want to " << std::endl + << " use personal images." << std::endl + << std::endl; exit(-1); } @@ -509,11 +498,9 @@ int main(int argc, const char **argv) vpCTRACE << "Load: " << filename << std::endl; grabber.open(Idisplay); grabber.acquire(Idisplay); - vpCameraParameters mcamTmp(592, 570, grabber.getWidth() / 2, - grabber.getHeight() / 2); + vpCameraParameters mcamTmp(592, 570, grabber.getWidth() / 2, grabber.getHeight() / 2); // Compute the initial pose of the camera - computeInitialPose(&mcamTmp, Idisplay, &mPose, md, mcog, &cMo, mP, - opt_click_allowed); + computeInitialPose(&mcamTmp, Idisplay, &mPose, md, mcog, &cMo, mP, opt_click_allowed); // Close the framegrabber grabber.close(); @@ -528,8 +515,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -539,8 +525,7 @@ int main(int argc, const char **argv) ogre.init(IC); ogre.load("Robot", "robot.mesh"); ogre.setScale("Robot", 0.001f, 0.001f, 0.001f); - ogre.setRotation("Robot", - vpRotationMatrix(vpRxyzVector(M_PI / 2, -M_PI / 2, 0))); + ogre.setRotation("Robot", vpRotationMatrix(vpRxyzVector(M_PI / 2, -M_PI / 2, 0))); // Add an optional point light source Ogre::Light *light = ogre.getSceneManager()->createLight(); @@ -596,8 +581,7 @@ 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; + std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl; return 1; } catch (...) { std::cout << "Catch an exception " << std::endl; diff --git a/example/parse-argv/parse-argv1.cpp b/example/parse-argv/parse-argv1.cpp index 3025d8d376..74a5d92c80 100644 --- a/example/parse-argv/parse-argv1.cpp +++ b/example/parse-argv/parse-argv1.cpp @@ -57,10 +57,8 @@ // List of allowed command line options #define GETOPTARGS "d:f:i:h" -void usage(const char *name, const char *badparam, int i_val, float f_val, - double d_val); -bool getOptions(int argc, const char **argv, int &i_val, float &f_val, - double &d_val); +void usage(const char *name, const char *badparam, int i_val, float f_val, double d_val); +bool getOptions(int argc, const char **argv, int &i_val, float &f_val, double &d_val); /*! @@ -73,8 +71,7 @@ bool getOptions(int argc, const char **argv, int &i_val, float &f_val, \param d_val : A double. */ -void usage(const char *name, const char *badparam, int i_val, float f_val, - double d_val) +void usage(const char *name, const char *badparam, int i_val, float f_val, double d_val) { fprintf(stdout, "\n\ Parsing command line arguments example.\n\ @@ -115,8 +112,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, int &i_val, float &f_val, - double &d_val) +bool getOptions(int argc, const char **argv, int &i_val, float &f_val, double &d_val) { const char *optarg_; int c; @@ -174,8 +170,7 @@ int main(int argc, const char **argv) cout << " Integer value: " << i_val << endl; cout << " Float value: " << f_val << endl; cout << " Double value: " << d_val << endl << endl; - cout << "Call " << argv[0] - << " -h to see how to change these parameters." << endl; + cout << "Call " << argv[0] << " -h to see how to change these parameters." << endl; return 0; } catch (vpException &e) { diff --git a/example/parse-argv/parse-argv2.cpp b/example/parse-argv/parse-argv2.cpp index eff441b57d..bb5b137bab 100644 --- a/example/parse-argv/parse-argv2.cpp +++ b/example/parse-argv/parse-argv2.cpp @@ -69,26 +69,17 @@ int main(int argc, const char **argv) char *string_val = NULL; vpParseArgv::vpArgvInfo argTable[] = { - {"-bool", vpParseArgv::ARGV_CONSTANT_BOOL, 0, (char *)&bool_val, - "Bool enabled."}, - {"-integer", vpParseArgv::ARGV_INT, (char *)NULL, (char *)&int_val, - "An integer value."}, - {"-long", vpParseArgv::ARGV_LONG, (char *)NULL, (char *)&long_val, - "A long value."}, - {"-float", vpParseArgv::ARGV_FLOAT, (char *)NULL, (char *)&float_val, - "A float value."}, - {"-double", vpParseArgv::ARGV_DOUBLE, (char *)NULL, - (char *)&double_val, "A double value."}, - {"-string", vpParseArgv::ARGV_STRING, (char *)NULL, - (char *)&string_val, "A chain value."}, - {"-h", vpParseArgv::ARGV_HELP, (char *)NULL, (char *)NULL, - "Print the help."}, - {(char *)NULL, vpParseArgv::ARGV_END, (char *)NULL, (char *)NULL, - (char *)NULL}}; + {"-bool", vpParseArgv::ARGV_CONSTANT_BOOL, 0, (char *)&bool_val, "Bool enabled."}, + {"-integer", vpParseArgv::ARGV_INT, (char *)NULL, (char *)&int_val, "An integer value."}, + {"-long", vpParseArgv::ARGV_LONG, (char *)NULL, (char *)&long_val, "A long value."}, + {"-float", vpParseArgv::ARGV_FLOAT, (char *)NULL, (char *)&float_val, "A float value."}, + {"-double", vpParseArgv::ARGV_DOUBLE, (char *)NULL, (char *)&double_val, "A double value."}, + {"-string", vpParseArgv::ARGV_STRING, (char *)NULL, (char *)&string_val, "A chain 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_DEFAULTS)) { + if (vpParseArgv::parse(&argc, argv, argTable, vpParseArgv::ARGV_NO_DEFAULTS)) { return (-1); } @@ -103,13 +94,11 @@ int main(int argc, const char **argv) else cout << " String value: \"\"" << endl << endl; - cout << "Call " << argv[0] - << " -h to see how to change these parameters." << endl; + cout << "Call " << argv[0] << " -h to see how to change these parameters." << endl; return 0; } catch (const vpException &e) { - std::cout << "Catch a ViSP exception: " << e.getStringMessage() - << std::endl; + std::cout << "Catch a ViSP exception: " << e.getStringMessage() << std::endl; return 1; } } diff --git a/example/pose-estimation/poseVirtualVS.cpp b/example/pose-estimation/poseVirtualVS.cpp index ca3018c3b4..fea358371d 100644 --- a/example/pose-estimation/poseVirtualVS.cpp +++ b/example/pose-estimation/poseVirtualVS.cpp @@ -65,8 +65,7 @@ #include #include #include -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) #include #include @@ -86,11 +85,9 @@ // List of allowed command line options #define GETOPTARGS "cdi:p:hf:n:s:" -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath, unsigned first, unsigned nimages, - unsigned step); -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &ppath, unsigned &first, unsigned &nimages, +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first, + unsigned nimages, unsigned step); +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages, unsigned &step, bool &click_allowed, bool &display); /*! @@ -106,8 +103,8 @@ Print the program options. \param step : Step between two images. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath, unsigned first, unsigned nimages, unsigned step) +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first, + unsigned nimages, unsigned step) { fprintf(stdout, "\n\ Test dot tracking.\n\ @@ -178,8 +175,7 @@ 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, - std::string &ppath, unsigned &first, unsigned &nimages, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages, unsigned &step, bool &click_allowed, bool &display) { const char *optarg_; @@ -248,15 +244,13 @@ int main(int argc, const char **argv) int i; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " poseVirtualVS.cpp" << std::endl << std::endl; std::cout << " Example of dots tracking in an image sequence and pose " "computation" << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH @@ -268,8 +262,8 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, - opt_step, opt_click_allowed, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_click_allowed, + opt_display) == false) { exit(-1); } @@ -283,26 +277,20 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " 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); + usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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 option if you want to " - << std::endl - << " use personal images" << std::endl - << std::endl; + std::cerr << " Use -i 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 option if you want to " << std::endl + << " use personal images" << std::endl + << std::endl; exit(-1); } @@ -376,13 +364,11 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable" << std::endl; } else { std::cerr << std::endl << "ERROR:" << std::endl; std::cerr << " Cannot read " << filename << std::endl; - std::cerr << " or your -p " << opt_ppath << " option " << std::endl - << std::endl; + std::cerr << " or your -p " << opt_ppath << " option " << std::endl << std::endl; } exit(-1); } @@ -415,8 +401,7 @@ int main(int argc, const char **argv) vpImagePoint cog[4]; // Center of gravity of the dot if (opt_display && opt_click_allowed) { // dot coordinates (u,v) = (column,row) - std::cout << "Click the four white dots on the object corner clockwise" - << std::endl; + std::cout << "Click the four white dots on the object corner clockwise" << std::endl; for (i = 0; i < 4; i++) { // tracking is initalized if no other parameters are given // to the iniTracking(..) method a right mouse click on the diff --git a/example/robot-simulator/afma6/servoSimuAfma6FourPoints2DCamVelocity.cpp b/example/robot-simulator/afma6/servoSimuAfma6FourPoints2DCamVelocity.cpp index cc969c69d2..e6795a54f4 100644 --- a/example/robot-simulator/afma6/servoSimuAfma6FourPoints2DCamVelocity.cpp +++ b/example/robot-simulator/afma6/servoSimuAfma6FourPoints2DCamVelocity.cpp @@ -56,10 +56,8 @@ #include #include -#if ((defined(_WIN32) && !defined(WINRT_8_0)) || \ - defined(VISP_HAVE_PTHREAD)) && \ - (defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || \ - defined(VISP_HAVE_GDI)) +#if ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) && \ + (defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI)) // We need to use threading capabilities. Thus on Unix-like // platforms, the libpthread third-party library need to be @@ -88,8 +86,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -138,8 +135,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -213,21 +209,16 @@ int main(int argc, const char **argv) vpServo task; std::cout << std::endl; - std::cout << "----------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, articular velocity are computed" - << std::endl; + std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo 4 points " << std::endl; - std::cout << "----------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------" << std::endl; std::cout << std::endl; // sets the initial camera location - vpHomogeneousMatrix cMo(-0.05, -0.05, 0.7, vpMath::rad(10), - vpMath::rad(10), vpMath::rad(-30)); + vpHomogeneousMatrix cMo(-0.05, -0.05, 0.7, vpMath::rad(10), vpMath::rad(10), vpMath::rad(-30)); // sets the point coordinates in the object frame vpPoint point[4]; @@ -244,15 +235,13 @@ int main(int argc, const char **argv) // sets the desired position of the point vpFeaturePoint p[4]; for (unsigned int i = 0; i < 4; i++) - vpFeatureBuilder::create( - p[i], point[i]); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure // sets the desired position of the feature point s* vpFeaturePoint pd[4]; // Desired pose - vpHomogeneousMatrix cdMo(vpHomogeneousMatrix( - 0.0, 0.0, 0.8, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0))); + vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0, 0.0, 0.8, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0))); // Projection of the points for (unsigned int i = 0; i < 4; i++) @@ -278,13 +267,11 @@ int main(int argc, const char **argv) vpSimulatorAfma6 robot(opt_display); // Initialise the robot and especially the camera - robot.init(vpAfma6::TOOL_CCMOP, - vpCameraParameters::perspectiveProjWithoutDistortion); + robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion); robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); // Initialise the object for the display part*/ - robot.initScene(vpWireFrameSimulator::PLATE, - vpWireFrameSimulator::D_STANDARD); + robot.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD); // Initialise the position of the object relative to the pose of the // robot's camera @@ -310,8 +297,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; vpTRACE("\t loop"); while (iter++ < 500) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // Get the Time at the beginning of the loop @@ -321,8 +307,7 @@ int main(int argc, const char **argv) cMo = robot.get_cMo(); if (iter == 1) { - std::cout - << "Initial robot position with respect to the object frame:\n"; + std::cout << "Initial robot position with respect to the object frame:\n"; cMo.print(); } @@ -342,8 +327,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed && iter == 1) { // suppressed for automate test - std::cout << "Click in the internal view window to continue..." - << std::endl; + std::cout << "Click in the internal view window to continue..." << std::endl; vpDisplay::getClick(Iint); } @@ -353,8 +337,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || " << (task.getError()).sumSquare() << std::endl; // The main loop has a duration of 10 ms at minimum vpTime::wait(t, 10); @@ -369,8 +352,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed) { // suppressed for automate test - std::cout << "Click in the internal view window to end..." - << std::endl; + std::cout << "Click in the internal view window to end..." << std::endl; vpDisplay::getClick(Iint); } return 0; diff --git a/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocity.cpp b/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocity.cpp index 4708d22eda..18197a9fc5 100644 --- a/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocity.cpp +++ b/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocity.cpp @@ -185,16 +185,12 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : 3D visual servoing " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // Sets the initial camera location @@ -259,8 +255,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // Start the visual servoing loop. We stop the servo after 200 iterations while (iter++ < 200) { - std::cout << "------------------------------------" << iter - << std::endl; + std::cout << "------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -286,8 +281,7 @@ int main(int argc, const char **argv) robot.setVelocity(vpRobot::CAMERA_FRAME, v); // Retrieve the error - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; // Save log flog << v.t() << " " << (task.getError()).t() << std::endl; diff --git a/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp b/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp index ad42683803..76f586261f 100644 --- a/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp +++ b/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp @@ -208,16 +208,12 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : 3D visual servoing " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // Sets the initial camera location @@ -243,10 +239,8 @@ int main(int argc, const char **argv) // From the camera desired pose build the corresponding homogeneous matrix vpHomogeneousMatrix cdMo(cd_r_o); - vpHomogeneousMatrix - cMcd; // Transformation between current and desired camera frame - vpRotationMatrix - cRcd; // Rotation between current and desired camera frame + vpHomogeneousMatrix cMcd; // Transformation between current and desired camera frame + vpRotationMatrix cRcd; // Rotation between current and desired camera frame // Set the constant gain of the servo double lambda = 1; @@ -254,8 +248,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // Start the visual servoing loop. We stop the servo after 200 iterations while (iter++ < 200) { - std::cout << "------------------------------------" << iter - << std::endl; + std::cout << "------------------------------------" << iter << std::endl; // get the robot position robot.getPosition(wMc); @@ -294,12 +287,10 @@ int main(int argc, const char **argv) robot.setVelocity(vpRobot::CAMERA_FRAME, velocity); // Retrieve the error (s-s*) - std::cout << "|| s - s* || = " << ctcd.t() << " " << tu_cRcd.t() - << std::endl; + std::cout << "|| s - s* || = " << ctcd.t() << " " << tu_cRcd.t() << std::endl; // Save log - flog << velocity.t() << " " << ctcd.t() << " " << tu_cRcd.t() - << std::endl; + flog << velocity.t() << " " << ctcd.t() << " " << tu_cRcd.t() << std::endl; } // Close the log file diff --git a/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocity.cpp b/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocity.cpp index 8eb9eb18bd..ce241f266f 100644 --- a/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocity.cpp +++ b/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocity.cpp @@ -186,16 +186,12 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : 3D visual servoing " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // Sets the initial camera location @@ -283,8 +279,7 @@ int main(int argc, const char **argv) robot.setVelocity(vpRobot::CAMERA_FRAME, v); // Retrieve the error - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; // Save log flog << v.t() << " " << (task.getError()).t() << std::endl; diff --git a/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp b/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp index cd7c0175da..81e632ab85 100644 --- a/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp +++ b/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp @@ -200,17 +200,12 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; - std::cout << " Test program without vpServo and vpFeature classes " - << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; + std::cout << " Test program without vpServo and vpFeature classes " << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : 3D visual servoing " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // Sets the initial camera location @@ -235,12 +230,9 @@ int main(int argc, const char **argv) // From the camera desired pose build the corresponding homogeneous matrix vpHomogeneousMatrix cdMo(cd_r_o); - vpHomogeneousMatrix - cdMc; // Transformation between desired and current camera frame - vpRotationMatrix - cdRc; // Rotation between desired and current camera frame - vpRotationMatrix - cRcd; // Rotation between current and desired camera frame + vpHomogeneousMatrix cdMc; // Transformation between desired and current camera frame + vpRotationMatrix cdRc; // Rotation between desired and current camera frame + vpRotationMatrix cRcd; // Rotation between current and desired camera frame // Set the constant gain of the servo double lambda = 1; @@ -286,12 +278,10 @@ int main(int argc, const char **argv) robot.setVelocity(vpRobot::CAMERA_FRAME, velocity); // Retrieve the error (s-s*) - std::cout << "|| s - s* || = " << cdtc.t() << " " << tu_cdRc.t() - << std::endl; + std::cout << "|| s - s* || = " << cdtc.t() << " " << tu_cdRc.t() << std::endl; // Save log - flog << velocity.t() << " " << cdtc.t() << " " << tu_cdRc.t() - << std::endl; + flog << velocity.t() << " " << cdtc.t() << " " << tu_cdRc.t() << std::endl; } // Close the log file flog.close(); diff --git a/example/robot-simulator/camera/servoSimuCircle2DCamVelocity.cpp b/example/robot-simulator/camera/servoSimuCircle2DCamVelocity.cpp index 36c6f6da22..80c8469fdf 100644 --- a/example/robot-simulator/camera/servoSimuCircle2DCamVelocity.cpp +++ b/example/robot-simulator/camera/servoSimuCircle2DCamVelocity.cpp @@ -152,13 +152,11 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo a circle " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // sets the initial camera location @@ -211,8 +209,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 500) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -230,8 +227,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information diff --git a/example/robot-simulator/camera/servoSimuCircle2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuCircle2DCamVelocityDisplay.cpp index 1c41cf68df..785497e172 100644 --- a/example/robot-simulator/camera/servoSimuCircle2DCamVelocityDisplay.cpp +++ b/example/robot-simulator/camera/servoSimuCircle2DCamVelocityDisplay.cpp @@ -50,8 +50,7 @@ #include #include -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) #include #include @@ -76,8 +75,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -129,8 +127,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -218,14 +215,12 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; // sets the initial camera location - vpHomogeneousMatrix cMo(0, 0, 1, vpMath::rad(0), vpMath::rad(80), - vpMath::rad(30)); + vpHomogeneousMatrix cMo(0, 0, 1, vpMath::rad(0), vpMath::rad(80), vpMath::rad(30)); vpHomogeneousMatrix wMc, wMo; robot.getPosition(wMc); wMo = wMc * cMo; // Compute the position of the object in the world frame - vpHomogeneousMatrix cMod(-0.1, -0.1, 0.7, vpMath::rad(40), - vpMath::rad(10), vpMath::rad(30)); + vpHomogeneousMatrix cMod(-0.1, -0.1, 0.7, vpMath::rad(40), vpMath::rad(10), vpMath::rad(30)); // sets the circle coordinates in the world frame vpCircle circle; @@ -258,8 +253,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 200) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -286,8 +280,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information @@ -305,10 +298,6 @@ int main(int argc, const char **argv) } } #else -int main() -{ - vpERROR_TRACE( - "You do not have X11, GTK, GDI or OpenCV display functionalities..."); -} +int main() { vpERROR_TRACE("You do not have X11, GTK, GDI or OpenCV display functionalities..."); } #endif diff --git a/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplay.cpp index 2dea56b4aa..d3abcd57e6 100644 --- a/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplay.cpp +++ b/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplay.cpp @@ -50,8 +50,7 @@ #include #include -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) #include #include @@ -76,8 +75,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -126,8 +124,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -215,16 +212,14 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; // sets the initial camera location - vpHomogeneousMatrix cMo(-0.2, 0.1, 2, vpMath::rad(5), vpMath::rad(5), - vpMath::rad(20)); + vpHomogeneousMatrix cMo(-0.2, 0.1, 2, vpMath::rad(5), vpMath::rad(5), vpMath::rad(20)); vpHomogeneousMatrix wMc, wMo; robot.getPosition(wMc); wMo = wMc * cMo; // Compute the position of the object in the world frame // sets the final camera location (for simulation purpose) - vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(-60), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(-60), vpMath::rad(0), vpMath::rad(0)); // sets the cylinder coordinates in the world frame vpCylinder cylinder(0, 1, 0, // direction @@ -275,8 +270,7 @@ int main(int argc, const char **argv) task.print(); if (opt_display && opt_click_allowed) { - std::cout << "\n\nClick in the camera view window to start..." - << std::endl; + std::cout << "\n\nClick in the camera view window to start..." << std::endl; vpDisplay::getClick(I); } @@ -289,8 +283,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop do { - std::cout << "---------------------------------------------" << iter++ - << std::endl; + std::cout << "---------------------------------------------" << iter++ << std::endl; vpColVector v; // get the robot position @@ -319,8 +312,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; ; // vpDisplay::getClick(I) ; @@ -342,10 +334,6 @@ int main(int argc, const char **argv) } #else -int main() -{ - vpERROR_TRACE( - "You do not have X11, GTK, GDI or OpenCV display functionalities..."); -} +int main() { vpERROR_TRACE("You do not have X11, GTK, GDI or OpenCV display functionalities..."); } #endif diff --git a/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp b/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp index 835bd73bb9..98e8f99dc1 100644 --- a/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp +++ b/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp @@ -53,8 +53,7 @@ #include -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) #include #include @@ -80,8 +79,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -130,8 +128,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -201,8 +198,7 @@ int main(int argc, const char **argv) // Display size is automatically defined by the image (Iint) and // (Iext) size displayInt.init(Iint, 100, 100, "Internal view"); - displayExt.init(Iext, (int)(130 + Iint.getWidth()), 100, - "External view"); + displayExt.init(Iext, (int)(130 + Iint.getWidth()), 100, "External view"); // Display the image // The image class has a member that specify a pointer toward // the display that has been initialized in the display declaration @@ -232,16 +228,14 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; // sets the initial camera location - vpHomogeneousMatrix cMo(-0.2, 0.1, 2, vpMath::rad(5), vpMath::rad(5), - vpMath::rad(20)); + vpHomogeneousMatrix cMo(-0.2, 0.1, 2, vpMath::rad(5), vpMath::rad(5), vpMath::rad(20)); vpHomogeneousMatrix wMc, wMo; robot.getPosition(wMc); wMo = wMc * cMo; // Compute the position of the object in the world frame // sets the final camera location (for simulation purpose) - vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); // sets the cylinder coordinates in the world frame vpCylinder cylinder(0, 1, 0, // direction @@ -292,8 +286,7 @@ int main(int argc, const char **argv) task.addFeature(l[1], ld[1]); // Set the point of view of the external view - vpHomogeneousMatrix cextMo(0, 0, 6, vpMath::rad(40), vpMath::rad(10), - vpMath::rad(60)); + vpHomogeneousMatrix cextMo(0, 0, 6, vpMath::rad(40), vpMath::rad(10), vpMath::rad(60)); // Display the initial scene vpServoDisplay::display(task, cam, Iint); @@ -305,8 +298,7 @@ int main(int argc, const char **argv) task.print(); if (opt_display && opt_click_allowed) { - std::cout << "\n\nClick in the internal camera view window to start..." - << std::endl; + std::cout << "\n\nClick in the internal camera view window to start..." << std::endl; vpDisplay::getClick(Iint); } @@ -319,8 +311,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // The first loop is needed to reach the desired position do { - std::cout << "---------------------------------------------" << iter++ - << std::endl; + std::cout << "---------------------------------------------" << iter++ << std::endl; vpColVector v; // get the robot position @@ -356,8 +347,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } while ((task.getError()).sumSquare() > 1e-9); // Second loop is to compute the control law while taking into account the @@ -443,15 +433,13 @@ int main(int argc, const char **argv) robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; iter++; } if (opt_display && opt_click_allowed) { - std::cout << "\nClick in the internal camera view window to end..." - << std::endl; + std::cout << "\nClick in the internal camera view window to end..." << std::endl; vpDisplay::getClick(Iint); } @@ -468,11 +456,6 @@ int main(int argc, const char **argv) #else #include -int main() -{ - std::cout - << "You do not have X11, GTK, GDI or OpenCV display functionalities..." - << std::endl; -} +int main() { std::cout << "You do not have X11, GTK, GDI or OpenCV display functionalities..." << std::endl; } #endif diff --git a/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocity.cpp b/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocity.cpp index ba97f3ea89..ee3b885d2e 100644 --- a/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocity.cpp +++ b/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocity.cpp @@ -154,15 +154,12 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout << " Eye-in-hand task control, articular velocity are computed" - << std::endl; + std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo 4 points " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // sets the initial camera location with respect to the object @@ -191,8 +188,7 @@ int main(int argc, const char **argv) // sets the desired position of the point vpFeaturePoint p[4]; for (i = 0; i < 4; i++) - vpFeatureBuilder::create( - p[i], point[i]); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure // sets the desired position of the point vpFeaturePoint pd[4]; @@ -231,8 +227,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 1500) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // Set the Jacobian (expressed in the end-effector frame) @@ -264,8 +259,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller ") ; robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information diff --git a/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocityDisplay.cpp index ca7b211036..18bb512ffa 100644 --- a/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocityDisplay.cpp +++ b/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocityDisplay.cpp @@ -55,8 +55,7 @@ #include -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) #include #include @@ -81,8 +80,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -131,8 +129,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -218,29 +215,24 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "----------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout << " Eye-in-hand task control, articular velocity are computed" - << std::endl; + std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo 4 points " << std::endl; - std::cout << "----------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------" << std::endl; std::cout << std::endl; // sets the initial camera location - vpHomogeneousMatrix cMo(-0.1, -0.1, 1, vpMath::rad(40), vpMath::rad(10), - vpMath::rad(60)); + vpHomogeneousMatrix cMo(-0.1, -0.1, 1, vpMath::rad(40), vpMath::rad(10), vpMath::rad(60)); // Compute the position of the object in the world frame vpHomogeneousMatrix wMc, wMo; robot.getPosition(wMc); wMo = wMc * cMo; - vpHomogeneousMatrix cextMo( - 0, 0, 2, 0, 0, - 0); // vpMath::rad(40), vpMath::rad(10), vpMath::rad(60)) ; + vpHomogeneousMatrix cextMo(0, 0, 2, 0, 0, + 0); // vpMath::rad(40), vpMath::rad(10), vpMath::rad(60)) ; // sets the point coordinates in the object frame vpPoint point[4]; @@ -260,8 +252,7 @@ int main(int argc, const char **argv) // sets the desired position of the point vpFeaturePoint p[4]; for (i = 0; i < 4; i++) - vpFeatureBuilder::create( - p[i], point[i]); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure // sets the desired position of the feature point s* vpFeaturePoint pd[4]; @@ -300,8 +291,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 200) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // Set the Jacobian (expressed in the end-effector frame) @@ -342,8 +332,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information @@ -355,8 +344,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed) { // suppressed for automate test - std::cout << "\n\nClick in the internal view window to end..." - << std::endl; + std::cout << "\n\nClick in the internal view window to end..." << std::endl; vpDisplay::getClick(Iint); } return 0; @@ -368,11 +356,6 @@ int main(int argc, const char **argv) #else #include -int main() -{ - std::cout - << "You do not have X11, GTK, GDI or OpenCV display functionalities..." - << std::endl; -} +int main() { std::cout << "You do not have X11, GTK, GDI or OpenCV display functionalities..." << std::endl; } #endif diff --git a/example/robot-simulator/camera/servoSimuFourPoints2DPolarCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuFourPoints2DPolarCamVelocityDisplay.cpp index 52b47f41c6..c14b22ff86 100644 --- a/example/robot-simulator/camera/servoSimuFourPoints2DPolarCamVelocityDisplay.cpp +++ b/example/robot-simulator/camera/servoSimuFourPoints2DPolarCamVelocityDisplay.cpp @@ -56,8 +56,7 @@ #include #include -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) #include #include @@ -85,8 +84,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -135,8 +133,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -257,15 +254,12 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "----------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout << " Eye-in-hand task control, articular velocity are computed" - << std::endl; + std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo 4 points " << std::endl; - std::cout << "----------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------" << std::endl; std::cout << std::endl; // #define TRANS_Z_PURE @@ -277,51 +271,39 @@ int main(int argc, const char **argv) #if defined(TRANS_Z_PURE) // sets the initial camera location - vpHomogeneousMatrix cMo(0, 0, 3, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMo(0, 0, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); // sets the desired camera location - vpHomogeneousMatrix cMod(0, 0, 2, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMod(0, 0, 2, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); #elif defined(TRANS_X_PURE) // sets the initial camera location - vpHomogeneousMatrix cMo(0.3, 0.3, 3, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMo(0.3, 0.3, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); // sets the desired camera location - vpHomogeneousMatrix cMod(0.5, 0.3, 3, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMod(0.5, 0.3, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); #elif defined(ROT_Z_PURE) // sets the initial camera location - vpHomogeneousMatrix cMo(0, 0, 3, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMo(0, 0, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); // sets the desired camera location - vpHomogeneousMatrix cMod(0, 0, 3, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(180)); + vpHomogeneousMatrix cMod(0, 0, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(180)); #elif defined(ROT_X_PURE) // sets the initial camera location - vpHomogeneousMatrix cMo(0, 0, 3, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMo(0, 0, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); // sets the desired camera location - vpHomogeneousMatrix cMod(0, 0, 3, vpMath::rad(45), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMod(0, 0, 3, vpMath::rad(45), vpMath::rad(0), vpMath::rad(0)); #elif defined(COMPLEX) // sets the initial camera location - vpHomogeneousMatrix cMo(0.2, 0.2, 3, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMo(0.2, 0.2, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); // sets the desired camera location - vpHomogeneousMatrix cMod(0, 0, 2.5, vpMath::rad(45), vpMath::rad(10), - vpMath::rad(30)); + vpHomogeneousMatrix cMod(0, 0, 2.5, vpMath::rad(45), vpMath::rad(10), vpMath::rad(30)); #elif defined(PROBLEM) // Bad behavior with an interaction matrix computed from the desired // features sets the initial camera location - vpHomogeneousMatrix cMo(0.2, 0.2, 3, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMo(0.2, 0.2, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); // sets the desired camera location - vpHomogeneousMatrix cMod(0.4, 0.2, 3, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMod(0.4, 0.2, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); #endif // Compute the position of the object in the world frame @@ -329,8 +311,7 @@ int main(int argc, const char **argv) robot.getPosition(wMc); wMo = wMc * cMo; - vpHomogeneousMatrix cextMo(0, 0, 6, vpMath::rad(40), vpMath::rad(10), - vpMath::rad(60)); + vpHomogeneousMatrix cextMo(0, 0, 6, vpMath::rad(40), vpMath::rad(10), vpMath::rad(60)); // sets the point coordinates in the object frame vpPoint point[4]; @@ -398,8 +379,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 200) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // Set the Jacobian (expressed in the end-effector frame) @@ -445,20 +425,17 @@ int main(int argc, const char **argv) // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to camera translation velocities in m/s // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; std::cout << "v: " << v.t() << std::endl; - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; // Save feature error (s-s*) for the 4 feature points. For each feature // point, we have 2 errors (along x and y axis). This error is // expressed in meters in the camera frame flog << (task.getError()).t() << " "; // s-s* for point 4 - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; // Save current visual feature s = (rho,theta) for (i = 0; i < 4; i++) { @@ -475,10 +452,8 @@ int main(int argc, const char **argv) ip.set_i(10); ip.set_j(10); - std::cout << "\nClick in the internal camera view to continue..." - << std::endl; - vpDisplay::displayText(Iint, ip, "A click to continue...", - vpColor::red); + std::cout << "\nClick in the internal camera view to continue..." << std::endl; + vpDisplay::displayText(Iint, ip, "A click to continue...", vpColor::red); vpDisplay::flush(Iint); vpDisplay::getClick(Iint); } @@ -507,11 +482,6 @@ int main(int argc, const char **argv) } } #else -int main() -{ - std::cout - << "You do not have X11, GTK, GDI or OpenCV display functionalities..." - << std::endl; -} +int main() { std::cout << "You do not have X11, GTK, GDI or OpenCV display functionalities..." << std::endl; } #endif diff --git a/example/robot-simulator/camera/servoSimuLine2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuLine2DCamVelocityDisplay.cpp index d2d6ec51d0..52032fd6c4 100644 --- a/example/robot-simulator/camera/servoSimuLine2DCamVelocityDisplay.cpp +++ b/example/robot-simulator/camera/servoSimuLine2DCamVelocityDisplay.cpp @@ -48,8 +48,7 @@ #include #include -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) #include #include @@ -74,8 +73,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -125,8 +123,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -214,8 +211,7 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; // sets the initial camera location - vpHomogeneousMatrix cMo(-0.2, 0.1, 1, vpMath::rad(5), vpMath::rad(5), - vpMath::rad(90)); + vpHomogeneousMatrix cMo(-0.2, 0.1, 1, vpMath::rad(5), vpMath::rad(5), vpMath::rad(90)); // Compute the position of the object in the world frame vpHomogeneousMatrix wMc, wMo; @@ -223,8 +219,7 @@ int main(int argc, const char **argv) wMo = wMc * cMo; // sets the final camera location (for simulation purpose) - vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); // sets the line coordinates (2 planes) in the world frame vpColVector plane1(4); @@ -275,16 +270,14 @@ int main(int argc, const char **argv) task.print(); if (opt_display && opt_click_allowed) { - std::cout << "\n\nClick in the camera view window to start..." - << std::endl; + std::cout << "\n\nClick in the camera view window to start..." << std::endl; vpDisplay::getClick(I); } unsigned int iter = 0; // loop while (iter++ < 200) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -309,8 +302,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } if (opt_display && opt_click_allowed) { @@ -329,11 +321,6 @@ int main(int argc, const char **argv) } #else -int main() -{ - std::cout - << "You do not have X11, GTK, GDI or OpenCV display functionalities..." - << std::endl; -} +int main() { std::cout << "You do not have X11, GTK, GDI or OpenCV display functionalities..." << std::endl; } #endif diff --git a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity1.cpp b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity1.cpp index 6425768bcb..acfb0ed9c8 100644 --- a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity1.cpp +++ b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity1.cpp @@ -162,8 +162,7 @@ int main(int argc, const char **argv) // 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; @@ -187,8 +186,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 100) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -207,8 +205,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information diff --git a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity2.cpp b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity2.cpp index fdd50a7554..f3a815b0db 100644 --- a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity2.cpp +++ b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity2.cpp @@ -151,15 +151,12 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout << " Eye-in-hand task control, articular velocity are computed" - << std::endl; + std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl; 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 @@ -181,8 +178,7 @@ int main(int argc, const char **argv) // 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; @@ -215,8 +211,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 100) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // Set the Jacobian (expressed in the end-effector frame) @@ -231,11 +226,10 @@ int main(int argc, const char **argv) // new point position point.track(cMo); - vpFeatureBuilder::create( - p, point); // retrieve x,y and Z of the vpPoint structure - pd.buildFrom(0, 0, 1); // Since vpServo::MEAN interaction matrix is - // used, we need to update the desired feature at - // each iteration + vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure + pd.buildFrom(0, 0, 1); // Since vpServo::MEAN interaction matrix is + // used, we need to update the desired feature at + // each iteration // compute the control law v = task.computeControlLaw(); @@ -243,8 +237,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information diff --git a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity3.cpp b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity3.cpp index 9d4b46fcfc..133134c291 100644 --- a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity3.cpp +++ b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity3.cpp @@ -150,15 +150,12 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout << " Eye-in-hand task control, articular velocity are computed" - << std::endl; + std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl; 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 @@ -180,8 +177,7 @@ int main(int argc, const char **argv) // 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; @@ -214,8 +210,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 100) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // Set the Jacobian (expressed in the end-effector frame) @@ -230,8 +225,7 @@ int main(int argc, const char **argv) // new point position point.track(cMo); - 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 // compute the control law v = task.computeControlLaw(); @@ -239,8 +233,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information diff --git a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity1.cpp b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity1.cpp index 32c5c8863c..e19a1ace58 100644 --- a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity1.cpp +++ b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity1.cpp @@ -150,17 +150,14 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; std::cout << " task : 2 1/2 D visual servoing " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // sets the initial camera location - vpPoseVector c_r_o(0.1, 0.2, 2, vpMath::rad(20), vpMath::rad(10), - vpMath::rad(50)); + vpPoseVector c_r_o(0.1, 0.2, 2, vpMath::rad(20), vpMath::rad(10), vpMath::rad(50)); vpHomogeneousMatrix cMo(c_r_o); // Compute the position of the object in the world frame @@ -169,8 +166,7 @@ int main(int argc, const char **argv) wMo = wMc * cMo; // sets the desired camera location - vpPoseVector cd_r_o(0, 0, 1, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpPoseVector cd_r_o(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); vpHomogeneousMatrix cdMo(cd_r_o); // sets the point coordinates in the world frame @@ -194,13 +190,11 @@ int main(int argc, const char **argv) // 2nd feature (Z) // not necessary to project twice (reuse p) vpFeaturePoint3D Z; - vpFeatureBuilder::create( - Z, point); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(Z, point); // retrieve x,y and Z of the vpPoint structure // want to see it one meter away (here again use pd) vpFeaturePoint3D Zd; - vpFeatureBuilder::create( - Zd, pointd); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(Zd, pointd); // retrieve x,y and Z of the vpPoint structure //------------------------------------------------------------------ // 3rd feature ThetaU @@ -233,8 +227,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 200) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -255,8 +248,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller ") ; robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information diff --git a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity2.cpp b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity2.cpp index 8885c63449..770858a779 100644 --- a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity2.cpp +++ b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity2.cpp @@ -143,11 +143,9 @@ int main(int argc, const char **argv) } std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " simulation of a 2 1/2 D visual servoing " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // In this example we will simulate a visual servoing task. @@ -161,8 +159,7 @@ int main(int argc, const char **argv) // sets the initial camera location // we give the camera location as a size 6 vector (3 translations in meter // and 3 rotation (theta U representation) - vpPoseVector c_r_o(0.1, 0.2, 2, vpMath::rad(20), vpMath::rad(10), - vpMath::rad(50)); + vpPoseVector c_r_o(0.1, 0.2, 2, vpMath::rad(20), vpMath::rad(10), vpMath::rad(50)); // this pose vector is then transformed in a 4x4 homogeneous matrix vpHomogeneousMatrix cMo(c_r_o); @@ -181,8 +178,7 @@ int main(int argc, const char **argv) // let us defined the defined camera location. // It is defined by cdMo // sets the desired camera location - vpPoseVector cd_r_o(0, 0, 1, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpPoseVector cd_r_o(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); vpHomogeneousMatrix cdMo(cd_r_o); //---------------------------------------------------------------------- @@ -308,8 +304,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 200) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -341,8 +336,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller ") ; robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information diff --git a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity3.cpp b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity3.cpp index 565529a38c..0c6cf54861 100644 --- a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity3.cpp +++ b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity3.cpp @@ -143,11 +143,9 @@ int main(int argc, const char **argv) } std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " simulation of a 2 1/2 D visual servoing " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // In this example we will simulate a visual servoing task. @@ -161,8 +159,7 @@ int main(int argc, const char **argv) // sets the initial camera location // we give the camera location as a size 6 vector (3 translations in meter // and 3 rotation (theta U representation) - vpPoseVector c_r_o(0.1, 0.2, 2, vpMath::rad(20), vpMath::rad(10), - vpMath::rad(50)); + vpPoseVector c_r_o(0.1, 0.2, 2, vpMath::rad(20), vpMath::rad(10), vpMath::rad(50)); // this pose vector is then transformed in a 4x4 homogeneous matrix vpHomogeneousMatrix cMo(c_r_o); @@ -181,8 +178,7 @@ int main(int argc, const char **argv) // let us defined the defined camera location. // It is defined by cdMo // sets the desired camera location " ) ; - vpPoseVector cd_r_o(0, 0, 1, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpPoseVector cd_r_o(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); vpHomogeneousMatrix cdMo(cd_r_o); //---------------------------------------------------------------------- @@ -296,8 +292,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 200) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -319,8 +314,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information diff --git a/example/robot-simulator/camera/servoSimuPoint3DCamVelocity.cpp b/example/robot-simulator/camera/servoSimuPoint3DCamVelocity.cpp index 67cd86cb90..9485b45e57 100644 --- a/example/robot-simulator/camera/servoSimuPoint3DCamVelocity.cpp +++ b/example/robot-simulator/camera/servoSimuPoint3DCamVelocity.cpp @@ -145,16 +145,12 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo a 3D point " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // sets the initial camera location @@ -200,8 +196,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 200) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -220,8 +215,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information diff --git a/example/robot-simulator/camera/servoSimuSphere2DCamVelocity.cpp b/example/robot-simulator/camera/servoSimuSphere2DCamVelocity.cpp index fc5eca1aef..9319fe876b 100644 --- a/example/robot-simulator/camera/servoSimuSphere2DCamVelocity.cpp +++ b/example/robot-simulator/camera/servoSimuSphere2DCamVelocity.cpp @@ -145,13 +145,11 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo a sphere " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // sets the initial camera location @@ -201,8 +199,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 500) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -221,8 +218,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information diff --git a/example/robot-simulator/camera/servoSimuSphere2DCamVelocitySecondaryTask.cpp b/example/robot-simulator/camera/servoSimuSphere2DCamVelocitySecondaryTask.cpp index dc5a3c12bf..a35095b269 100644 --- a/example/robot-simulator/camera/servoSimuSphere2DCamVelocitySecondaryTask.cpp +++ b/example/robot-simulator/camera/servoSimuSphere2DCamVelocitySecondaryTask.cpp @@ -147,13 +147,11 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo a sphere with a secondary task" << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // sets the initial camera location @@ -204,8 +202,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 500) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -236,8 +233,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; } // Display task information diff --git a/example/robot-simulator/camera/servoSimuSquareLine2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuSquareLine2DCamVelocityDisplay.cpp index f0d38ad798..7f87faa709 100644 --- a/example/robot-simulator/camera/servoSimuSquareLine2DCamVelocityDisplay.cpp +++ b/example/robot-simulator/camera/servoSimuSquareLine2DCamVelocityDisplay.cpp @@ -48,8 +48,7 @@ #include #include -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) #include #include @@ -74,8 +73,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -125,8 +123,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -215,8 +212,7 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; // sets the initial camera location - vpHomogeneousMatrix cMo(0.2, 0.2, 1, vpMath::rad(45), vpMath::rad(45), - vpMath::rad(125)); + vpHomogeneousMatrix cMo(0.2, 0.2, 1, vpMath::rad(45), vpMath::rad(45), vpMath::rad(125)); // Compute the position of the object in the world frame vpHomogeneousMatrix wMc, wMo; @@ -224,8 +220,7 @@ int main(int argc, const char **argv) wMo = wMc * cMo; // sets the final camera location (for simulation purpose) - vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); int nbline = 4; @@ -282,16 +277,14 @@ int main(int argc, const char **argv) task.print(); if (opt_display && opt_click_allowed) { - std::cout << "\n\nClick in the camera view window to start..." - << std::endl; + std::cout << "\n\nClick in the camera view window to start..." << std::endl; vpDisplay::getClick(I); } unsigned int iter = 0; // loop while (iter++ < 200) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -317,8 +310,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; ; } @@ -338,11 +330,6 @@ int main(int argc, const char **argv) } #else -int main() -{ - std::cout - << "You do not have X11, GTK, GDI or OpenCV display functionalities..." - << std::endl; -} +int main() { std::cout << "You do not have X11, GTK, GDI or OpenCV display functionalities..." << std::endl; } #endif diff --git a/example/robot-simulator/camera/servoSimuThetaUCamVelocity.cpp b/example/robot-simulator/camera/servoSimuThetaUCamVelocity.cpp index 0961883b47..1e05ac6a08 100644 --- a/example/robot-simulator/camera/servoSimuThetaUCamVelocity.cpp +++ b/example/robot-simulator/camera/servoSimuThetaUCamVelocity.cpp @@ -145,21 +145,16 @@ int main(int argc, const char **argv) vpSimulatorCamera robot; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo using theta U visual feature " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // sets the initial camera location - vpPoseVector c_r_o(0.1, 0.2, 2, vpMath::rad(20), vpMath::rad(10), - vpMath::rad(50)); + vpPoseVector c_r_o(0.1, 0.2, 2, vpMath::rad(20), vpMath::rad(10), vpMath::rad(50)); vpHomogeneousMatrix cMo(c_r_o); // Compute the position of the object in the world frame @@ -168,8 +163,7 @@ int main(int argc, const char **argv) wMo = wMc * cMo; // sets the desired camera location - vpPoseVector cd_r_o(0, 0, 1, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpPoseVector cd_r_o(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); vpHomogeneousMatrix cdMo(cd_r_o); // compute the rotation that the camera has to realize @@ -195,8 +189,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 200) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // get the robot position @@ -214,8 +207,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; ; } diff --git a/example/robot-simulator/viper850/servoSimuViper850FourPoints2DCamVelocity.cpp b/example/robot-simulator/viper850/servoSimuViper850FourPoints2DCamVelocity.cpp index 0829f70ec7..32289d971b 100644 --- a/example/robot-simulator/viper850/servoSimuViper850FourPoints2DCamVelocity.cpp +++ b/example/robot-simulator/viper850/servoSimuViper850FourPoints2DCamVelocity.cpp @@ -56,10 +56,8 @@ #include #include -#if ((defined(_WIN32) && !defined(WINRT_8_0)) || \ - defined(VISP_HAVE_PTHREAD)) && \ - (defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || \ - defined(VISP_HAVE_GDI)) +#if ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) && \ + (defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GDI)) // We need to use threading capabilities. Thus on Unix-like // platforms, the libpthread third-party library need to be @@ -88,8 +86,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -138,8 +135,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -214,21 +210,16 @@ int main(int argc, const char **argv) vpServo task; std::cout << std::endl; - std::cout << "----------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, articular velocity are computed" - << std::endl; + std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo 4 points " << std::endl; - std::cout << "----------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------" << std::endl; std::cout << std::endl; // sets the initial camera location - vpHomogeneousMatrix cMo(-0.05, -0.05, 0.7, vpMath::rad(10), - vpMath::rad(10), vpMath::rad(-30)); + vpHomogeneousMatrix cMo(-0.05, -0.05, 0.7, vpMath::rad(10), vpMath::rad(10), vpMath::rad(-30)); // sets the point coordinates in the object frame vpPoint point[4]; @@ -245,15 +236,13 @@ int main(int argc, const char **argv) // sets the desired position of the point vpFeaturePoint p[4]; for (unsigned int i = 0; i < 4; i++) - vpFeatureBuilder::create( - p[i], point[i]); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure // sets the desired position of the feature point s* vpFeaturePoint pd[4]; // Desired pose - vpHomogeneousMatrix cdMo(vpHomogeneousMatrix( - 0.0, 0.0, 0.8, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0))); + vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0, 0.0, 0.8, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0))); // Projection of the points for (unsigned int i = 0; i < 4; i++) @@ -279,13 +268,11 @@ int main(int argc, const char **argv) vpSimulatorViper850 robot(opt_display); // Initialise the robot and especially the camera - robot.init(vpViper850::TOOL_PTGREY_FLEA2_CAMERA, - vpCameraParameters::perspectiveProjWithoutDistortion); + robot.init(vpViper850::TOOL_PTGREY_FLEA2_CAMERA, vpCameraParameters::perspectiveProjWithoutDistortion); robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); // Initialise the object for the display part - robot.initScene(vpWireFrameSimulator::PLATE, - vpWireFrameSimulator::D_STANDARD); + robot.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD); // Initialise the position of the object relative to the pose of the // robot's camera @@ -311,8 +298,7 @@ int main(int argc, const char **argv) unsigned int iter = 0; // loop while (iter++ < 500) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; vpColVector v; // Get the Time at the beginning of the loop @@ -322,8 +308,7 @@ int main(int argc, const char **argv) cMo = robot.get_cMo(); if (iter == 1) { - std::cout - << "Initial robot position with respect to the object frame:\n"; + std::cout << "Initial robot position with respect to the object frame:\n"; cMo.print(); } @@ -343,8 +328,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed && iter == 1) { // suppressed for automate test - std::cout << "Click in the internal view window to continue..." - << std::endl; + std::cout << "Click in the internal view window to continue..." << std::endl; vpDisplay::getClick(Iint); } @@ -354,8 +338,7 @@ int main(int argc, const char **argv) // send the camera velocity to the controller robot.setVelocity(vpRobot::CAMERA_FRAME, v); - std::cout << "|| s - s* || " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || " << (task.getError()).sumSquare() << std::endl; // The main loop has a duration of 10 ms at minimum vpTime::wait(t, 10); @@ -370,8 +353,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed) { // suppressed for automate test - std::cout << "Click in the internal view window to end..." - << std::endl; + std::cout << "Click in the internal view window to end..." << std::endl; vpDisplay::getClick(Iint); } return 0; diff --git a/example/servo-afma4/moveAfma4.cpp b/example/servo-afma4/moveAfma4.cpp index b69cd6ccd4..2ba8ef9b2d 100644 --- a/example/servo-afma4/moveAfma4.cpp +++ b/example/servo-afma4/moveAfma4.cpp @@ -251,8 +251,7 @@ int main(int argc, const char **argv) #else int main() { - vpERROR_TRACE( - "You do not have an afma4 robot connected to your computer..."); + vpERROR_TRACE("You do not have an afma4 robot connected to your computer..."); return 0; } diff --git a/example/servo-afma4/servoAfma4Point2DArtVelocity.cpp b/example/servo-afma4/servoAfma4Point2DArtVelocity.cpp index 41aeb214e0..51f40b324d 100644 --- a/example/servo-afma4/servoAfma4Point2DArtVelocity.cpp +++ b/example/servo-afma4/servoAfma4Point2DArtVelocity.cpp @@ -152,16 +152,12 @@ int main() // exit(1) ; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the joint space" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the joint space" << std::endl; std::cout << " Use of the Afma4 robot " << std::endl; std::cout << " task : servo a point " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; vpDot dot; @@ -180,8 +176,7 @@ int main() vpTRACE("sets the current position of the visual feature "); vpFeaturePoint p; - vpFeatureBuilder::create( - p, cam, dot); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure p.set_Z(1); vpTRACE("sets the desired position of the visual feature "); @@ -263,8 +258,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to joint translation velocities in m/s // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -274,8 +268,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -285,8 +278,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the feature point. For this feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-afma4/servoAfma4Point2DCamVelocity.cpp b/example/servo-afma4/servoAfma4Point2DCamVelocity.cpp index e231439308..b2649e4f8a 100644 --- a/example/servo-afma4/servoAfma4Point2DCamVelocity.cpp +++ b/example/servo-afma4/servoAfma4Point2DCamVelocity.cpp @@ -145,16 +145,12 @@ int main() vpDisplay::flush(I); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; 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; vpDot dot; @@ -172,8 +168,7 @@ int main() vpTRACE("sets the current position of the visual feature "); vpFeaturePoint p; - vpFeatureBuilder::create( - p, cam, dot); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure vpTRACE("sets the desired position of the visual feature "); vpFeaturePoint pd; @@ -229,8 +224,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to camera translation velocities in m/s // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -240,8 +234,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -251,8 +244,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the feature point. For this feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-afma4/servoAfma4Point2DCamVelocityKalman.cpp b/example/servo-afma4/servoAfma4Point2DCamVelocityKalman.cpp index cec64834c4..eddd152dad 100644 --- a/example/servo-afma4/servoAfma4Point2DCamVelocityKalman.cpp +++ b/example/servo-afma4/servoAfma4Point2DCamVelocityKalman.cpp @@ -140,8 +140,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, KalmanType &kalman, - bool &doAdaptativeGain, +bool getOptions(int argc, const char **argv, KalmanType &kalman, bool &doAdaptativeGain, vpAdaptiveGain &lambda) // gain lambda { const char *optarg; @@ -189,8 +188,7 @@ int main(int argc, const char **argv) int opt_cam_frequency = 60; // 60 Hz // Read the command line options - if (getOptions(argc, argv, opt_kalman, doAdaptativeGain, lambda) == - false) { + if (getOptions(argc, argv, opt_kalman, doAdaptativeGain, lambda) == false) { return (-1); } @@ -258,35 +256,28 @@ int main(int argc, const char **argv) vpDisplay::flush(I); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << "Test program for target motion compensation using a Kalman " "filter " << std::endl; - std::cout - << "Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << "Eye-in-hand task control, velocity computed in the camera frame" << std::endl; std::cout << "Task : servo a point \n" << std::endl; // Kalman filtering switch (opt_kalman) { case K_NONE: - std::cout - << "Servo with no target motion compensation (see -K option)\n"; + std::cout << "Servo with no target motion compensation (see -K option)\n"; break; case K_VELOCITY: - std::cout - << "Servo with target motion compensation using a Kalman filter\n" - << "with constant velocity modelization (see -K option)\n"; + std::cout << "Servo with target motion compensation using a Kalman filter\n" + << "with constant velocity modelization (see -K option)\n"; break; case K_ACCELERATION: - std::cout - << "Servo with target motion compensation using a Kalman filter\n" - << "with constant acceleration modelization (see -K option)\n"; + std::cout << "Servo with target motion compensation using a Kalman filter\n" + << "with constant acceleration modelization (see -K option)\n"; break; } - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; vpDot2 dot; @@ -348,21 +339,19 @@ int main(int argc, const char **argv) switch (opt_kalman) { case K_VELOCITY: { // Set the constant velocity state model used for the filtering - kalman.setStateModel(vpLinearKalmanFilterInstantiation:: - stateConstVelWithColoredNoise_MeasureVel); + kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel); state_size = kalman.getStateSize(); sigma_state.resize(state_size * nsignal); sigma_state = 0.00001; // Same state variance for all signals sigma_measure = 0.05; // Same measure variance for all the signals - double dummy = 0; // non used parameter dt for the velocity state model + double dummy = 0; // non used parameter dt for the velocity state model kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy); break; } case K_ACCELERATION: { // Set the constant acceleration state model used for the filtering - kalman.setStateModel(vpLinearKalmanFilterInstantiation:: - stateConstAccWithColoredNoise_MeasureVel); + kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstAccWithColoredNoise_MeasureVel); state_size = kalman.getStateSize(); sigma_state.resize(state_size * nsignal); sigma_state = 0.00001; // Same variance for all the signals @@ -498,8 +487,7 @@ int main(int argc, const char **argv) // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to camera translation velocities in m/s // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Save feature error (s-s*) for the feature point. For this feature // point, we have 2 errors (along x and y axis). This error is @@ -507,8 +495,7 @@ int main(int argc, const char **argv) flog << task.error[0] << " " << task.error[1] << " "; // Save feature error (s-s*) in pixels in the image. - flog << cog.get_u() - cam.get_u0() << " " << cog.get_v() - cam.get_v0() - << " "; + flog << cog.get_u() - cam.get_u0() << " " << cog.get_v() - cam.get_v0() << " "; // Save de/dt flog << dedt_mes[0] << " " << dedt_mes[1] << " "; diff --git a/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp b/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp index 7fb233fbe6..1a41a61001 100644 --- a/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp +++ b/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp @@ -129,16 +129,12 @@ int main() robot.getCameraParameters(cam, I); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo a line " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; int nbline = 4; @@ -196,8 +192,7 @@ int main() for (i = 0; i < nbline; i++) { double x = 0, y = 0; - if (!vpMeLine::intersection(line[i % nbline], line[(i + 1) % nbline], - ip)) { + if (!vpMeLine::intersection(line[i % nbline], line[(i + 1) % nbline], ip)) { exit(-1); } @@ -240,8 +235,7 @@ int main() // The second feature is the depth of the current square center relative // to the depth of the desired square center. vpFeatureDepth logZ; - logZ.buildFrom(pointc.get_x(), pointc.get_y(), pointc.get_Z(), - log(pointc.get_Z() / pointcd.get_Z())); + logZ.buildFrom(pointc.get_x(), pointc.get_y(), pointc.get_Z(), log(pointc.get_Z() / pointcd.get_Z())); // The last three features are the rotations thetau between the current // pose and the desired pose. @@ -279,8 +273,7 @@ int main() double beta = 3; for (;;) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; try { g.acquire(I); @@ -296,8 +289,7 @@ int main() double x = 0, y = 0; - if (!vpMeLine::intersection(line[i % nbline], - line[(i + 1) % nbline], ip)) { + if (!vpMeLine::intersection(line[i % nbline], line[(i + 1) % nbline], ip)) { exit(-1); } @@ -328,8 +320,7 @@ int main() pointd[i].display(I, cam, vpColor::red); // Update the second feature - logZ.buildFrom(pointc.get_x(), pointc.get_y(), pointc.get_Z(), - log(pointc.get_Z() / pointcd.get_Z())); + logZ.buildFrom(pointc.get_x(), pointc.get_y(), pointc.get_Z(), log(pointc.get_Z() / pointcd.get_Z())); // Update the last three features cdMc = cMod * cMo.inverse(); @@ -341,8 +332,7 @@ int main() if (std::fabs(alpha) <= std::numeric_limits::epsilon()) gain = lambda_av; else { - gain = alpha * exp(-beta * (task.getError()).sumSquare()) + - lambda_av; + gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av; } } diff --git a/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp index 6b6a676672..b799376805 100644 --- a/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp +++ b/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp @@ -105,16 +105,12 @@ int main() vpServo task; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; 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; int i; @@ -193,8 +189,7 @@ int main() double alpha = 0.2; double beta = 3; for (;;) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; try { g.acquire(I); @@ -220,8 +215,7 @@ int main() if (std::fabs(alpha) <= std::numeric_limits::epsilon()) gain = lambda_av; else { - gain = alpha * exp(-beta * (task.getError()).sumSquare()) + - lambda_av; + gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av; } } task.setLambda(gain); diff --git a/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp b/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp index 1ebc807a9e..96b19a9401 100644 --- a/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp +++ b/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp @@ -110,16 +110,12 @@ int main() vpServo task; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; 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; int i; @@ -201,8 +197,7 @@ int main() // First loop to reach the convergence position while (erreur > 0.00001) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; try { g.acquire(I); @@ -227,8 +222,7 @@ int main() if (std::fabs(alpha) <= std::numeric_limits::epsilon()) gain = lambda_av; else { - gain = alpha * exp(-beta * (task.getError()).sumSquare()) + - lambda_av; + gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av; } } task.setLambda(gain); @@ -266,8 +260,7 @@ int main() unsigned int tempo = 1200; for (;;) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; try { g.acquire(I); diff --git a/example/servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp index 9859192883..68c590a4ba 100644 --- a/example/servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp +++ b/example/servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp @@ -108,16 +108,12 @@ int main() vpDisplay::flush(I); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; 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; vpDot dot; @@ -189,8 +185,7 @@ int main() std::cout << "beta 5" << std::endl; std::cin >> beta; for (;;) { - std::cout << "---------------------------------------------" << iter++ - << std::endl; + std::cout << "---------------------------------------------" << iter++ << std::endl; g.acquire(I); vpDisplay::display(I); @@ -209,8 +204,7 @@ int main() if (std::fabs(alpha) <= std::numeric_limits::epsilon()) gain = lambda_av; else { - gain = - alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av; + gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av; } } else gain = lambda_av; diff --git a/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp b/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp index 6507c3dcff..636e7da704 100644 --- a/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp +++ b/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp @@ -154,25 +154,18 @@ int main() vpDisplay::flush(I); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the joint space" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the joint space" << std::endl; std::cout << " Use of the Afma6 robot " << std::endl; - std::cout << " task : servo 4 points on a square with dimention " << L - << " meters" << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; vpDot dot[4]; vpImagePoint cog; - std::cout - << "Click on the 4 dots clockwise starting from upper/left dot..." - << std::endl; + std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl; for (i = 0; i < 4; i++) { dot[i].initTracking(I); @@ -191,8 +184,7 @@ int main() // Sets the current position of the visual feature vpFeaturePoint p[4]; for (i = 0; i < 4; i++) - vpFeatureBuilder::create( - p[i], cam, dot[i]); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y and Z of the vpPoint structure // sets the desired position of the visual feature vpFeaturePoint pd[4]; @@ -284,8 +276,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to joint translation velocities in m/s // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -295,8 +286,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -306,8 +296,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the 4 feature points. For each feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp b/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp index 816f6c91b9..91901850df 100644 --- a/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp +++ b/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp @@ -119,8 +119,7 @@ Lagrange or Dementhon methods. */ -void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, - vpCameraParameters cam, vpHomogeneousMatrix &cMo, +void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, vpCameraParameters cam, vpHomogeneousMatrix &cMo, vpTranslationVector &cto, vpRxyzVector &cro, bool init) { vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon @@ -273,27 +272,19 @@ int main() vpDisplay::flush(I); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; std::cout << " Use of the Afma6 robot " << std::endl; - std::cout << " Interaction matrix computed with the current features " - << std::endl; - std::cout << " task : servo 4 points on a square with dimention " << L - << " meters" << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << " Interaction matrix computed with the current features " << std::endl; + std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; vpDot2 dot[4]; vpImagePoint cog; - std::cout - << "Click on the 4 dots clockwise starting from upper/left dot..." - << std::endl; + std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl; for (i = 0; i < 4; i++) { dot[i].initTracking(I); cog = dot[i].getCog(); @@ -301,8 +292,7 @@ int main() vpDisplay::flush(I); } - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithDistortion; + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithDistortion; vpRobotAfma6 robot; // Load the end-effector to camera frame transformation obtained @@ -316,8 +306,7 @@ int main() // Sets the current position of the visual feature vpFeaturePoint p[4]; for (i = 0; i < 4; i++) - vpFeatureBuilder::create( - p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure + vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure // Set the position of the square target in a frame which origin is // centered in the middle of the square @@ -425,8 +414,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to camera translation velocities in m/s // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -436,8 +424,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -447,8 +434,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the 4 feature points. For each feature // point, we have 2 errors (along x and y axis). This error is @@ -457,7 +443,7 @@ int main() // Save the current cMo pose: translations in meters, rotations (rx, ry, // rz) in radians - flog << cto[0] << " " << cto[1] << " " << cto[2] << " " // translation + flog << cto[0] << " " << cto[1] << " " << cto[2] << " " // translation << cro[0] << " " << cro[1] << " " << cro[2] << std::endl; // rot // Flush the display diff --git a/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp b/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp index e189236f32..7d4b0a4036 100644 --- a/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp +++ b/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp @@ -151,28 +151,20 @@ int main() vpDisplay::flush(I); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; std::cout << " Use of the Afma6 robot " << std::endl; - std::cout << " Interaction matrix computed with the desired features " - << std::endl; + std::cout << " Interaction matrix computed with the desired features " << std::endl; - std::cout << " task : servo 4 points on a square with dimention " << L - << " meters" << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; vpDot2 dot[4]; vpImagePoint cog; - std::cout - << "Click on the 4 dots clockwise starting from upper/left dot..." - << std::endl; + std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl; for (i = 0; i < 4; i++) { dot[i].initTracking(I); cog = dot[i].getCog(); @@ -182,8 +174,7 @@ int main() vpRobotAfma6 robot; - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithDistortion; + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithDistortion; // Load the end-effector to camera frame transformation obtained // using a camera intrinsic model with distortion @@ -196,8 +187,7 @@ int main() // Sets the current position of the visual feature vpFeaturePoint p[4]; for (i = 0; i < 4; i++) - vpFeatureBuilder::create( - p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure + vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure // Set the position of the square target in a frame which origin is // centered in the middle of the square @@ -289,8 +279,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to camera translation velocities in m/s // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -300,8 +289,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured camera rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -311,8 +299,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the 4 feature points. For each feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-afma6/servoAfma6Line2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Line2DCamVelocity.cpp index e6559c9fd5..c1eb50c3f1 100644 --- a/example/servo-afma6/servoAfma6Line2DCamVelocity.cpp +++ b/example/servo-afma6/servoAfma6Line2DCamVelocity.cpp @@ -106,16 +106,12 @@ int main() vpServo task; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo a line " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; vpMeLine line; @@ -176,8 +172,7 @@ int main() vpTRACE("\t loop"); vpColVector v; for (;;) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; try { g.acquire(I); diff --git a/example/servo-afma6/servoAfma6Point2DArtVelocity.cpp b/example/servo-afma6/servoAfma6Point2DArtVelocity.cpp index 5f76e54233..0a195c53b4 100644 --- a/example/servo-afma6/servoAfma6Point2DArtVelocity.cpp +++ b/example/servo-afma6/servoAfma6Point2DArtVelocity.cpp @@ -148,16 +148,12 @@ int main() // exit(1) ; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the joint space" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the joint space" << std::endl; std::cout << " Use of the Afma6 robot " << std::endl; std::cout << " task : servo a point " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; vpDot dot; @@ -178,8 +174,7 @@ int main() vpTRACE("sets the current position of the visual feature "); vpFeaturePoint p; - vpFeatureBuilder::create( - p, cam, dot); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure p.set_Z(1); vpTRACE("sets the desired position of the visual feature "); @@ -261,8 +256,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to joint translation velocities in m/s // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -272,8 +266,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -283,8 +276,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the feature point. For this feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-afma6/servoAfma6Point2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Point2DCamVelocity.cpp index df3d03e447..1d7db6740e 100644 --- a/example/servo-afma6/servoAfma6Point2DCamVelocity.cpp +++ b/example/servo-afma6/servoAfma6Point2DCamVelocity.cpp @@ -212,8 +212,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to camera translation velocities in m/s // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -223,8 +222,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -234,8 +232,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the feature point. For this feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp b/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp index 277af49ce3..83c0af0b11 100644 --- a/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp +++ b/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp @@ -116,14 +116,12 @@ int main() vpDisplay::flush(I); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; std::cout << " Eye-to-hand task control" << std::endl; 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; int nbPoint = 7; @@ -257,19 +255,16 @@ int main() std::list Lcog; vpImagePoint ip; while (error > convergence_threshold) { - std::cout << "---------------------------------------------" << iter++ - << std::endl; + std::cout << "---------------------------------------------" << iter++ << std::endl; g.acquire(I); vpDisplay::display(I); ip.set_i(265); ip.set_j(150); - vpDisplay::displayText(I, ip, "Eye-To-Hand Visual Servoing", - vpColor::green); + vpDisplay::displayText(I, ip, "Eye-To-Hand Visual Servoing", vpColor::green); ip.set_i(280); ip.set_j(150); - vpDisplay::displayText(I, ip, "IRISA-INRIA Rennes, Lagadic project", - vpColor::green); + vpDisplay::displayText(I, ip, "IRISA-INRIA Rennes, Lagadic project", vpColor::green); try { for (i = 0; i < nbPoint; i++) { dot[i].track(I); @@ -323,23 +318,20 @@ int main() if (std::fabs(alpha) <= std::numeric_limits::epsilon()) gain = lambda_av; else { - gain = - alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av; + gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av; } } else gain = lambda_av; if (SAVE == 1) gain = gain / 5; - vpTRACE("%f %f %f %f %f", alpha, beta, lambda_av, - (task.getError()).sumSquare(), gain); + vpTRACE("%f %f %f %f %f", alpha, beta, lambda_av, (task.getError()).sumSquare(), gain); task.setLambda(gain); v = task.computeControlLaw(); // display points trajectory - for (std::list::const_iterator it_cog = Lcog.begin(); - it_cog != Lcog.end(); ++it_cog) { + for (std::list::const_iterator it_cog = Lcog.begin(); it_cog != Lcog.end(); ++it_cog) { vpDisplay::displayPoint(I, *it_cog, vpColor::red); } vpServoDisplay::display(task, cam, I); diff --git a/example/servo-afma6/servoAfma6Segment2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Segment2DCamVelocity.cpp index 771d98d4d9..87f447c6e2 100644 --- a/example/servo-afma6/servoAfma6Segment2DCamVelocity.cpp +++ b/example/servo-afma6/servoAfma6Segment2DCamVelocity.cpp @@ -149,8 +149,7 @@ int main() seg.display(cam, I, vpColor::red); vpDisplay::flush(I); std::cout << "define the destination segment" << std::endl; - for (std::vector::iterator i = dot_d.begin(); i != dot_d.end(); - ++i) { + for (std::vector::iterator i = dot_d.begin(); i != dot_d.end(); ++i) { vpImagePoint ip; vpDisplay::getClick(I, ip); *i = vpDot(ip); @@ -189,8 +188,7 @@ int main() vpDisplay::display(I); // Achieve the tracking of the dot in the image - for (std::vector::iterator i = dot.begin(); i != dot.end(); - ++i) { + for (std::vector::iterator i = dot.begin(); i != dot.end(); ++i) { i->track(I); } diff --git a/example/servo-afma6/servoAfma6SquareLines2DCamVelocity.cpp b/example/servo-afma6/servoAfma6SquareLines2DCamVelocity.cpp index 3a467bc51c..3861a9cb51 100644 --- a/example/servo-afma6/servoAfma6SquareLines2DCamVelocity.cpp +++ b/example/servo-afma6/servoAfma6SquareLines2DCamVelocity.cpp @@ -113,16 +113,12 @@ int main() vpServo task; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; std::cout << " Simulation " << std::endl; std::cout << " task : servo a line " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; int i; @@ -219,8 +215,7 @@ int main() double beta = 3; for (;;) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; try { g.acquire(I); @@ -242,8 +237,7 @@ int main() if (std::fabs(alpha) <= std::numeric_limits::epsilon()) gain = lambda_av; else { - gain = alpha * exp(-beta * (task.getError()).sumSquare()) + - lambda_av; + gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av; } } diff --git a/example/servo-afma6/servoAfma6TwoLines2DCamVelocity.cpp b/example/servo-afma6/servoAfma6TwoLines2DCamVelocity.cpp index 6c8b4fabeb..e8d21bf75d 100644 --- a/example/servo-afma6/servoAfma6TwoLines2DCamVelocity.cpp +++ b/example/servo-afma6/servoAfma6TwoLines2DCamVelocity.cpp @@ -113,16 +113,12 @@ int main() vpServo task; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; 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; int i; @@ -210,8 +206,7 @@ int main() double beta = 3; for (;;) { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; try { g.acquire(I); @@ -236,8 +231,7 @@ int main() if (std::fabs(alpha) <= std::numeric_limits::epsilon()) gain = lambda_av; else { - gain = alpha * exp(-beta * (task.getError()).sumSquare()) + - lambda_av; + gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av; } } task.setLambda(gain); diff --git a/example/servo-biclops/moveBiclops.cpp b/example/servo-biclops/moveBiclops.cpp index eea8a25a4a..ea8747a497 100644 --- a/example/servo-biclops/moveBiclops.cpp +++ b/example/servo-biclops/moveBiclops.cpp @@ -167,12 +167,10 @@ int main(int argc, const char **argv) robot.getPosition(vpRobot::ARTICULAR_FRAME, qm); std::cout << "Position in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) - << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) - << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; q[0] = vpMath::rad(10); q[1] = vpMath::rad(20); @@ -184,12 +182,10 @@ int main(int argc, const char **argv) robot.getPosition(vpRobot::ARTICULAR_FRAME, qm); std::cout << "Position in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) - << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) - << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; std::cout << "Set STATE_VELOCITY_CONTROL" << std::endl; robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); @@ -200,8 +196,7 @@ int main(int argc, const char **argv) << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) - << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; qdot = 0; // qdot[0] = vpMath::rad(0.1) ; @@ -220,8 +215,7 @@ int main(int argc, const char **argv) << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) - << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; qdot = 0; // qdot[0] = vpMath::rad(0.1) ; @@ -240,8 +234,7 @@ int main(int argc, const char **argv) << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) - << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; qdot = 0; // qdot[0] = vpMath::rad(0.1) ; @@ -260,8 +253,7 @@ int main(int argc, const char **argv) << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) - << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; qdot = 0; qdot[0] = vpMath::rad(-5); @@ -281,8 +273,7 @@ int main(int argc, const char **argv) << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl; robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm); std::cout << "Velocity in the articular frame: " - << " pan: " << vpMath::deg(qm[0]) - << " tilt: " << vpMath::deg(qm[1]) << std::endl; + << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl; } catch (vpException &e) { std::cout << "Catch an exception: " << e.getMessage() << std::endl; } @@ -290,8 +281,7 @@ int main(int argc, const char **argv) #else int main() { - vpERROR_TRACE( - "You do not have a biclops robot connected to your computer..."); + vpERROR_TRACE("You do not have a biclops robot connected to your computer..."); return 0; } diff --git a/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp b/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp index b4314c0942..347fee5afb 100644 --- a/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp +++ b/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp @@ -61,8 +61,7 @@ #include #include // Debug trace #include -#if (defined(VISP_HAVE_BICLOPS) && \ - (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_DIRECTSHOW))) +#if (defined(VISP_HAVE_BICLOPS) && (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_DIRECTSHOW))) #ifdef VISP_HAVE_PTHREAD #include @@ -118,8 +117,7 @@ void signalCtrC(int /* signumber */) \param user : Username. */ -void usage(const char *name, const char *badparam, std::string &conf, - std::string &debugdir, std::string &user) +void usage(const char *name, const char *badparam, std::string &conf, std::string &debugdir, std::string &user) { fprintf(stdout, "\n\ Example of eye-in-hand control law. We control here a real robot, the biclops\n\ @@ -158,8 +156,7 @@ Set the program options. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &conf, - std::string &debugdir, std::string &user) +bool getOptions(int argc, const char **argv, std::string &conf, std::string &debugdir, std::string &user) { const char *optarg_; int c; @@ -198,16 +195,12 @@ bool getOptions(int argc, const char **argv, std::string &conf, int main(int argc, const char **argv) { std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; 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; try { @@ -225,8 +218,7 @@ int main(int argc, const char **argv) std::string opt_debugdir; // Set the default output path -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX opt_debugdir = "/tmp"; #elif defined(_WIN32) opt_debugdir = "C:/temp"; @@ -256,8 +248,7 @@ int main(int argc, const char **argv) usage(argv[0], NULL, opt_conf, debugdir, username); std::cerr << std::endl << "ERROR:" << std::endl; std::cerr << " Cannot create " << dirname << std::endl; - std::cerr << " Check your -d " << debugdir << " option " - << std::endl; + std::cerr << " Check your -d " << debugdir << " option " << std::endl; exit(-1); } } @@ -317,8 +308,7 @@ int main(int argc, const char **argv) vpDot dot; try { - std::cout << "Click on a dot to initialize the tracking..." - << std::endl; + std::cout << "Click on a dot to initialize the tracking..." << std::endl; dot.setGraphics(true); dot.initTracking(I); dot.track(I); @@ -332,8 +322,7 @@ int main(int argc, const char **argv) // sets the current position of the visual feature vpFeaturePoint p; - vpFeatureBuilder::create( - p, cam, dot); // retrieve x,y and Z of the vpPoint structure + 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 @@ -382,8 +371,7 @@ int main(int argc, const char **argv) for (;;) #endif { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; g.acquire(I); vpDisplay::display(I); @@ -410,14 +398,12 @@ int main(int argc, const char **argv) std::cout << "v: " << v.t(); robot.setVelocity(vpRobot::ARTICULAR_FRAME, v); - std::cout << "|| s - s* || = " << (task.getError()).sumSquare() - << std::endl; + std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl; { vpColVector s_minus_sStar(2); s_minus_sStar = task.s - task.sStar; - fprintf(fd, "%f %f %f %f %f\n", v[0], v[1], s_minus_sStar[0], - s_minus_sStar[1], (task.getError()).sumSquare()); + fprintf(fd, "%f %f %f %f %f\n", v[0], v[1], s_minus_sStar[0], s_minus_sStar[1], (task.getError()).sumSquare()); } } diff --git a/example/servo-franka/franka_echo_robot_state.cpp b/example/servo-franka/franka_echo_robot_state.cpp index 5f2e4a614d..6c26301e35 100644 --- a/example/servo-franka/franka_echo_robot_state.cpp +++ b/example/servo-franka/franka_echo_robot_state.cpp @@ -46,9 +46,5 @@ int main(int argc, char **argv) } #else -int main() -{ - std::cout << "This example needs libfranka to control Panda robot." - << std::endl; -} +int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; } #endif diff --git a/example/servo-franka/franka_execute_trajectory.cpp b/example/servo-franka/franka_execute_trajectory.cpp index 921a72e6dc..a98661f630 100644 --- a/example/servo-franka/franka_execute_trajectory.cpp +++ b/example/servo-franka/franka_execute_trajectory.cpp @@ -23,14 +23,11 @@ * https://frankaemika.github.io/docs for more details. */ -template -std::ostream &operator<<(std::ostream &ostream, const std::array &array) +template std::ostream &operator<<(std::ostream &ostream, const std::array &array) { ostream << "["; - std::copy(array.cbegin(), array.cend() - 1, - std::ostream_iterator(ostream, ",")); - std::copy(array.cend() - 1, array.cend(), - std::ostream_iterator(ostream)); + std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator(ostream, ",")); + std::copy(array.cend() - 1, array.cend(), std::ostream_iterator(ostream)); ostream << "]"; return ostream; } @@ -38,8 +35,7 @@ std::ostream &operator<<(std::ostream &ostream, const std::array &array) int main(int argc, char **argv) { if (argc != 4) { - std::cerr << "Usage: " << argv[0] - << " " << std::endl; + std::cerr << "Usage: " << argv[0] << " " << std::endl; return -1; } @@ -64,21 +60,17 @@ int main(int argc, char **argv) // Set additional parameters always before the control loop, NEVER in the // control loop! Set collision behavior. - robot.setCollisionBehavior({{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); + robot.setCollisionBehavior( + {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, + {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, + {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, + {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); // Set the joint impedance. robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}}); size_t index = 0; - robot.control([&](const franka::RobotState &robot_state, - franka::Duration time_step) -> franka::JointPositions { + robot.control([&](const franka::RobotState &robot_state, franka::Duration time_step) -> franka::JointPositions { states.push_back(robot_state); index += time_step.toMSec(); @@ -112,9 +104,5 @@ int main(int argc, char **argv) } #else -int main() -{ - std::cout << "This example needs libfranka to control Panda robot." - << std::endl; -} +int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; } #endif diff --git a/example/servo-franka/franka_generate_cartesian_pose_motion.cpp b/example/servo-franka/franka_generate_cartesian_pose_motion.cpp index d54f4ddc66..a392c6109b 100644 --- a/example/servo-franka/franka_generate_cartesian_pose_motion.cpp +++ b/example/servo-franka/franka_generate_cartesian_pose_motion.cpp @@ -25,8 +25,7 @@ int main(int argc, char **argv) { if (argc != 2) { - std::cerr << "Usage: ./generate_cartesian_pose_motion " - << std::endl; + std::cerr << "Usage: ./generate_cartesian_pose_motion " << std::endl; return -1; } @@ -35,21 +34,16 @@ int main(int argc, char **argv) // Set additional parameters always before the control loop, NEVER in the // control loop! Set collision behavior. - robot.setCollisionBehavior({{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); + robot.setCollisionBehavior( + {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, + {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, + {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, + {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); auto initial_pose = robot.readOnce().O_T_EE_d; double radius = 0.3; double time = 0.0; - robot.control([=, &time]( - const franka::RobotState &, - franka::Duration time_step) -> franka::CartesianPose { + robot.control([=, &time](const franka::RobotState &, franka::Duration time_step) -> franka::CartesianPose { time += time_step.toSec(); double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * time)); @@ -61,8 +55,7 @@ int main(int argc, char **argv) new_pose[14] += delta_z; if (time >= 10.0) { - std::cout << std::endl - << "Finished motion, shutting down example" << std::endl; + std::cout << std::endl << "Finished motion, shutting down example" << std::endl; return franka::MotionFinished(new_pose); } return new_pose; @@ -76,9 +69,5 @@ int main(int argc, char **argv) } #else -int main() -{ - std::cout << "This example needs libfranka to control Panda robot." - << std::endl; -} +int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; } #endif diff --git a/example/servo-franka/franka_generate_cartesian_velocity_motion.cpp b/example/servo-franka/franka_generate_cartesian_velocity_motion.cpp index 01b51df2fb..c056e31d7c 100644 --- a/example/servo-franka/franka_generate_cartesian_velocity_motion.cpp +++ b/example/servo-franka/franka_generate_cartesian_velocity_motion.cpp @@ -25,9 +25,7 @@ int main(int argc, char **argv) { if (argc != 2) { - std::cerr - << "Usage: ./generate_cartesian_velocity_motion " - << std::endl; + std::cerr << "Usage: ./generate_cartesian_velocity_motion " << std::endl; return -1; } try { @@ -38,49 +36,34 @@ int main(int argc, char **argv) robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}}); // Set the collision behavior. - std::array lower_torque_thresholds_nominal{ - {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}}; - std::array upper_torque_thresholds_nominal{ - {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}}; - std::array lower_torque_thresholds_acceleration{ - {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}}; - std::array upper_torque_thresholds_acceleration{ - {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}}; - std::array lower_force_thresholds_nominal{ - {30.0, 30.0, 30.0, 25.0, 25.0, 25.0}}; - std::array upper_force_thresholds_nominal{ - {40.0, 40.0, 40.0, 35.0, 35.0, 35.0}}; - std::array lower_force_thresholds_acceleration{ - {30.0, 30.0, 30.0, 25.0, 25.0, 25.0}}; - std::array upper_force_thresholds_acceleration{ - {40.0, 40.0, 40.0, 35.0, 35.0, 35.0}}; - robot.setCollisionBehavior( - lower_torque_thresholds_acceleration, - upper_torque_thresholds_acceleration, lower_torque_thresholds_nominal, - upper_torque_thresholds_nominal, lower_force_thresholds_acceleration, - upper_force_thresholds_acceleration, lower_force_thresholds_nominal, - upper_force_thresholds_nominal); + std::array lower_torque_thresholds_nominal{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}}; + std::array upper_torque_thresholds_nominal{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}}; + std::array lower_torque_thresholds_acceleration{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}}; + std::array upper_torque_thresholds_acceleration{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}}; + std::array lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}}; + std::array upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}}; + std::array lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}}; + std::array upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}}; + robot.setCollisionBehavior(lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration, + lower_torque_thresholds_nominal, upper_torque_thresholds_nominal, + lower_force_thresholds_acceleration, upper_force_thresholds_acceleration, + lower_force_thresholds_nominal, upper_force_thresholds_nominal); double time_max = 4.0; double v_max = 0.1; double angle = M_PI / 4.0; double time = 0.0; - robot.control([=, &time](const franka::RobotState &, - franka::Duration time_step) - -> franka::CartesianVelocities { + robot.control([=, &time](const franka::RobotState &, franka::Duration time_step) -> franka::CartesianVelocities { time += time_step.toSec(); - double cycle = std::floor( - pow(-1.0, (time - std::fmod(time, time_max)) / time_max)); - double v = cycle * v_max / 2.0 * - (1.0 - std::cos(2.0 * M_PI / time_max * time)); + double cycle = std::floor(pow(-1.0, (time - std::fmod(time, time_max)) / time_max)); + double v = cycle * v_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time)); double v_x = std::cos(angle) * v; double v_z = -std::sin(angle) * v; franka::CartesianVelocities output = {{v_x, 0.0, v_z, 0.0, 0.0, 0.0}}; if (time >= 2 * time_max) { - std::cout << std::endl - << "Finished motion, shutting down example" << std::endl; + std::cout << std::endl << "Finished motion, shutting down example" << std::endl; return franka::MotionFinished(output); } return output; @@ -94,9 +77,5 @@ int main(int argc, char **argv) } #else -int main() -{ - std::cout << "This example needs libfranka to control Panda robot." - << std::endl; -} +int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; } #endif diff --git a/example/servo-franka/franka_generate_consecutive_motions.cpp b/example/servo-franka/franka_generate_consecutive_motions.cpp index 9bd849c449..f64df6f918 100644 --- a/example/servo-franka/franka_generate_consecutive_motions.cpp +++ b/example/servo-franka/franka_generate_consecutive_motions.cpp @@ -25,8 +25,7 @@ int main(int argc, char **argv) { if (argc != 2) { - std::cerr << "Usage: ./generate_consecutive_motions " - << std::endl; + std::cerr << "Usage: ./generate_consecutive_motions " << std::endl; return -1; } @@ -35,14 +34,10 @@ int main(int argc, char **argv) // Set additional parameters always before the control loop, NEVER in the // control loop! Set collision behavior. - robot.setCollisionBehavior({{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, - {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, - {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, - {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, - {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, - {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, - {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, - {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}); + robot.setCollisionBehavior({{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, + {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, + {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, + {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}); for (int i = 0; i < 5; i++) { std::cout << "Executing motion." << std::endl; @@ -50,18 +45,13 @@ int main(int argc, char **argv) double time_max = 4.0; double omega_max = 0.2; double time = 0.0; - robot.control([=, &time](const franka::RobotState &, - franka::Duration time_step) - -> franka::JointVelocities { + robot.control([=, &time](const franka::RobotState &, franka::Duration time_step) -> franka::JointVelocities { time += time_step.toSec(); - double cycle = std::floor( - std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max)); - double omega = cycle * omega_max / 2.0 * - (1.0 - std::cos(2.0 * M_PI / time_max * time)); + double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max)); + double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time)); - franka::JointVelocities velocities = { - {0.0, 0.0, omega, 0.0, 0.0, 0.0, 0.0}}; + franka::JointVelocities velocities = {{0.0, 0.0, omega, 0.0, 0.0, 0.0, 0.0}}; if (time >= 2 * time_max) { std::cout << std::endl << "Finished motion." << std::endl; return franka::MotionFinished(velocities); @@ -85,9 +75,5 @@ int main(int argc, char **argv) } #else -int main() -{ - std::cout << "This example needs libfranka to control Panda robot." - << std::endl; -} +int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; } #endif diff --git a/example/servo-franka/franka_generate_joint_position_motion.cpp b/example/servo-franka/franka_generate_joint_position_motion.cpp index 74d0f66049..d4c71f2b0b 100644 --- a/example/servo-franka/franka_generate_joint_position_motion.cpp +++ b/example/servo-franka/franka_generate_joint_position_motion.cpp @@ -25,8 +25,7 @@ int main(int argc, char **argv) { if (argc != 2) { - std::cerr << "Usage: ./generate_joint_position_motion " - << std::endl; + std::cerr << "Usage: ./generate_joint_position_motion " << std::endl; return -1; } @@ -35,33 +34,25 @@ int main(int argc, char **argv) // Set additional parameters always before the control loop, NEVER in the // control loop! Set collision behavior. - robot.setCollisionBehavior({{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); + robot.setCollisionBehavior( + {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, + {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, + {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, + {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); auto initial_position = robot.readOnce().q_d; double time = 0.0; - robot.control([=, &time]( - const franka::RobotState &, - franka::Duration time_step) -> franka::JointPositions { + robot.control([=, &time](const franka::RobotState &, franka::Duration time_step) -> franka::JointPositions { time += time_step.toSec(); double delta_angle = M_PI / 8 * (1 - std::cos(M_PI / 5.0 * time)); - franka::JointPositions output = { - {initial_position[0], initial_position[1], initial_position[2], - initial_position[3] + delta_angle, - initial_position[4] + delta_angle, initial_position[5], - initial_position[6] + delta_angle}}; + franka::JointPositions output = {{initial_position[0], initial_position[1], initial_position[2], + initial_position[3] + delta_angle, initial_position[4] + delta_angle, + initial_position[5], initial_position[6] + delta_angle}}; if (time >= 10.0) { - std::cout << std::endl - << "Finished motion, shutting down example" << std::endl; + std::cout << std::endl << "Finished motion, shutting down example" << std::endl; return franka::MotionFinished(output); } return output; @@ -75,9 +66,5 @@ int main(int argc, char **argv) } #else -int main() -{ - std::cout << "This example needs libfranka to control Panda robot." - << std::endl; -} +int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; } #endif diff --git a/example/servo-franka/franka_generate_joint_velocity_motion.cpp b/example/servo-franka/franka_generate_joint_velocity_motion.cpp index 1c2772825a..1c5e8aedce 100644 --- a/example/servo-franka/franka_generate_joint_velocity_motion.cpp +++ b/example/servo-franka/franka_generate_joint_velocity_motion.cpp @@ -25,8 +25,7 @@ int main(int argc, char **argv) { if (argc != 2) { - std::cerr << "Usage: ./generate_joint_velocity_motion " - << std::endl; + std::cerr << "Usage: ./generate_joint_velocity_motion " << std::endl; return -1; } try { @@ -34,34 +33,25 @@ int main(int argc, char **argv) // Set additional parameters always before the control loop, NEVER in the // control loop! Set collision behavior. - robot.setCollisionBehavior({{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); + robot.setCollisionBehavior( + {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, + {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, + {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, + {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); double time_max = 4.0; double omega_max = 0.2; double time = 0.0; - robot.control([=, &time]( - const franka::RobotState &, - franka::Duration time_step) -> franka::JointVelocities { + robot.control([=, &time](const franka::RobotState &, franka::Duration time_step) -> franka::JointVelocities { time += time_step.toSec(); - double cycle = std::floor( - std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max)); - double omega = cycle * omega_max / 2.0 * - (1.0 - std::cos(2.0 * M_PI / time_max * time)); + double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max)); + double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time)); - franka::JointVelocities velocities = { - {0.0, 0.0, 0.0, omega, omega, omega, omega}}; + franka::JointVelocities velocities = {{0.0, 0.0, 0.0, omega, omega, omega, omega}}; if (time >= 2 * time_max) { - std::cout << std::endl - << "Finished motion, shutting down example" << std::endl; + std::cout << std::endl << "Finished motion, shutting down example" << std::endl; return franka::MotionFinished(velocities); } return velocities; @@ -75,9 +65,5 @@ int main(int argc, char **argv) } #else -int main() -{ - std::cout << "This example needs libfranka to control Panda robot." - << std::endl; -} +int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; } #endif diff --git a/example/servo-franka/franka_grasp_object.cpp b/example/servo-franka/franka_grasp_object.cpp index 190bf8975f..dc60262c4f 100644 --- a/example/servo-franka/franka_grasp_object.cpp +++ b/example/servo-franka/franka_grasp_object.cpp @@ -25,9 +25,7 @@ int main(int argc, char **argv) { if (argc != 4) { - std::cerr - << "Usage: ./grasp_object " - << std::endl; + std::cerr << "Usage: ./grasp_object " << std::endl; return -1; } @@ -51,9 +49,7 @@ int main(int argc, char **argv) // Check for the maximum grasping width. franka::GripperState gripper_state = gripper.readOnce(); if (gripper_state.max_width < grasping_width) { - std::cout - << "Object is too large for the current fingers on the gripper." - << std::endl; + std::cout << "Object is too large for the current fingers on the gripper." << std::endl; return -1; } @@ -64,8 +60,7 @@ int main(int argc, char **argv) } // Wait 3s and check afterwards, if the object is still grasped. - std::this_thread::sleep_for( - std::chrono::duration(3000)); + std::this_thread::sleep_for(std::chrono::duration(3000)); gripper_state = gripper.readOnce(); if (!gripper_state.is_grasped) { @@ -84,9 +79,5 @@ int main(int argc, char **argv) } #else -int main() -{ - std::cout << "This example needs libfranka to control Panda robot." - << std::endl; -} +int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; } #endif diff --git a/example/servo-franka/franka_joint_impedance_control.cpp b/example/servo-franka/franka_joint_impedance_control.cpp index 468cfeb884..b4feb37d99 100644 --- a/example/servo-franka/franka_joint_impedance_control.cpp +++ b/example/servo-franka/franka_joint_impedance_control.cpp @@ -22,14 +22,11 @@ namespace { -template -std::ostream &operator<<(std::ostream &ostream, const std::array &array) +template std::ostream &operator<<(std::ostream &ostream, const std::array &array) { ostream << "["; - std::copy(array.cbegin(), array.cend() - 1, - std::ostream_iterator(ostream, ",")); - std::copy(array.cend() - 1, array.cend(), - std::ostream_iterator(ostream)); + std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator(ostream, ",")); + std::copy(array.cend() - 1, array.cend(), std::ostream_iterator(ostream)); ostream << "]"; return ostream; } @@ -93,8 +90,7 @@ int main(int argc, char **argv) std::thread print_thread([print_rate, &print_data, &running]() { while (running) { // Sleep to achieve the desired print rate. - std::this_thread::sleep_for(std::chrono::milliseconds( - static_cast((1.0 / print_rate * 1000.0)))); + std::this_thread::sleep_for(std::chrono::milliseconds(static_cast((1.0 / print_rate * 1000.0)))); // Try to lock data to avoid read write collisions. if (print_data.mutex.try_lock() && print_data.has_data) { @@ -104,16 +100,13 @@ int main(int argc, char **argv) for (size_t i = 0; i < 7; ++i) { tau_d_actual[i] = print_data.tau_d_last[i] + print_data.gravity[i]; tau_error[i] = tau_d_actual[i] - print_data.robot_state.tau_J[i]; - error_rms += - std::sqrt(std::pow(tau_error[i], 2.0)) / tau_error.size(); + error_rms += std::sqrt(std::pow(tau_error[i], 2.0)) / tau_error.size(); } // Print data to console std::cout << "tau_error [Nm]: " << tau_error << std::endl << "tau_commanded [Nm]: " << tau_d_actual << std::endl - << "tau_measured [Nm]: " << print_data.robot_state.tau_J - << std::endl - << "root mean square of tau_error [Nm]: " << error_rms - << std::endl + << "tau_measured [Nm]: " << print_data.robot_state.tau_J << std::endl + << "root mean square of tau_error [Nm]: " << error_rms << std::endl << "-----------------------" << std::endl; print_data.has_data = false; print_data.mutex.unlock(); @@ -126,14 +119,11 @@ int main(int argc, char **argv) franka::Robot robot(argv[1]); // Set collision behavior. - robot.setCollisionBehavior({{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); + robot.setCollisionBehavior( + {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, + {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, + {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, + {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); // Load the kinematics and dynamics model. franka::Model model = robot.loadModel(); @@ -143,23 +133,18 @@ int main(int argc, char **argv) // Define callback function to send Cartesian pose goals to get inverse // kinematics solved. - std::function - cartesian_pose_callback = - [=, &time, &vel_current, &running, - &angle](const franka::RobotState & /*state*/, - franka::Duration period) -> franka::CartesianPose { + std::function cartesian_pose_callback = + [=, &time, &vel_current, &running, &angle](const franka::RobotState & /*state*/, + franka::Duration period) -> franka::CartesianPose { // Update time. time += period.toSec(); // Compute Cartesian velocity. if (vel_current < vel_max && time < run_time) { - vel_current += - period.toSec() * std::fabs(vel_max / acceleration_time); + vel_current += period.toSec() * std::fabs(vel_max / acceleration_time); } if (vel_current > 0.0 && time > run_time) { - vel_current -= - period.toSec() * std::fabs(vel_max / acceleration_time); + vel_current -= period.toSec() * std::fabs(vel_max / acceleration_time); } vel_current = std::fmax(vel_current, 0.0); vel_current = std::fmin(vel_current, vel_max); @@ -188,31 +173,24 @@ int main(int argc, char **argv) // Set gains for the joint impedance control. // Stiffness - const std::array k_gains = { - {1000.0, 1000.0, 1000.0, 1000.0, 500.0, 300.0, 100.0}}; + const std::array k_gains = {{1000.0, 1000.0, 1000.0, 1000.0, 500.0, 300.0, 100.0}}; // Damping - const std::array d_gains = { - {100.0, 100.0, 100.0, 100.0, 50.0, 30.0, 10.0}}; + const std::array d_gains = {{100.0, 100.0, 100.0, 100.0, 50.0, 30.0, 10.0}}; // Define callback for the joint torque control loop. - std::function - impedance_control_callback = - [&print_data, &model, k_gains, - d_gains](const franka::RobotState &state, - franka::Duration /*period*/) -> franka::Torques { + std::function impedance_control_callback = + [&print_data, &model, k_gains, d_gains](const franka::RobotState &state, + franka::Duration /*period*/) -> franka::Torques { // Read current coriolis terms from model. - std::array coriolis = model.coriolis( - state, {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}, 0.0, - {{0.0, 0.0, 0.0}}); + std::array coriolis = + model.coriolis(state, {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}, 0.0, {{0.0, 0.0, 0.0}}); // Compute torque command from joint impedance control law. // Note: The answer to our Cartesian pose inverse kinematics is always // in state.q_d with one time step delay. std::array tau_d; for (size_t i = 0; i < 7; i++) { - tau_d[i] = k_gains[i] * (state.q_d[i] - state.q[i]) - - d_gains[i] * state.dq[i] + coriolis[i]; + tau_d[i] = k_gains[i] * (state.q_d[i] - state.q[i]) - d_gains[i] * state.dq[i] + coriolis[i]; } // Update data to print. @@ -243,9 +221,5 @@ int main(int argc, char **argv) } #else -int main() -{ - std::cout << "This example needs libfranka to control Panda robot." - << std::endl; -} +int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; } #endif diff --git a/example/servo-franka/franka_joint_point_to_point_motion.cpp b/example/servo-franka/franka_joint_point_to_point_motion.cpp index 68c750ec7d..30f2349863 100644 --- a/example/servo-franka/franka_joint_point_to_point_motion.cpp +++ b/example/servo-franka/franka_joint_point_to_point_motion.cpp @@ -36,8 +36,7 @@ inline int sgn(double x) return (x > 0) ? 1 : -1; } -std::array add(const std::array &a, - const std::array &b) +std::array add(const std::array &a, const std::array &b) { std::array result; for (size_t i = 0; i < a.size(); i++) { @@ -46,8 +45,7 @@ std::array add(const std::array &a, return result; } -std::array subtract(const std::array &a, - const std::array &b) +std::array subtract(const std::array &a, const std::array &b) { std::array result; for (size_t i = 0; i < a.size(); i++) { @@ -56,30 +54,22 @@ std::array subtract(const std::array &a, return result; } -bool calculateDesiredValues(double t, const std::array &delta_q, - const std::array &dq_max, - const std::array &t_1, - const std::array &t_2, - const std::array &t_f, - const std::array &q_1, +bool calculateDesiredValues(double t, const std::array &delta_q, const std::array &dq_max, + const std::array &t_1, const std::array &t_2, + const std::array &t_f, const std::array &q_1, std::array *delta_q_d); -void calculateSynchronizedValues(const std::array &delta_q, - const std::array &dq_max, - const std::array &ddq_max_start, - const std::array &ddq_max_goal, - std::array *dq_max_sync, - std::array *t_1_sync, - std::array *t_2_sync, - std::array *t_f_sync, +void calculateSynchronizedValues(const std::array &delta_q, const std::array &dq_max, + const std::array &ddq_max_start, const std::array &ddq_max_goal, + std::array *dq_max_sync, std::array *t_1_sync, + std::array *t_2_sync, std::array *t_f_sync, std::array *q_1); int main(int argc, char **argv) { if (argc != 10) { std::cerr << "Usage: ./generate_joint_pose_motion " - << " " - << std::endl + << " " << std::endl << "speed-factor must be between zero and one." << std::endl; return -1; } @@ -94,14 +84,11 @@ int main(int argc, char **argv) // Set additional parameters always before the control loop, NEVER in the // control loop! Set collision behavior. - robot.setCollisionBehavior({{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, - {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, - {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, - {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, - {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, - {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, - {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, - {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}); + robot.setCollisionBehavior( + {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, + {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, + {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, + {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}); std::array q_start = robot.readOnce().q_d; @@ -123,23 +110,19 @@ int main(int argc, char **argv) std::array q_1{}; std::array delta_q = subtract(q_goal, q_start); - calculateSynchronizedValues(delta_q, dq_max, ddq_max_start, ddq_max_goal, - &dq_max_sync, &t_1_sync, &t_2_sync, &t_f_sync, - &q_1); - robot.control( - [=, &time](const franka::RobotState &, - franka::Duration time_step) -> franka::JointPositions { - time += time_step.toSec(); - - std::array delta_q_d; - bool motion_finished = - calculateDesiredValues(time, delta_q, dq_max_sync, t_1_sync, - t_2_sync, t_f_sync, q_1, &delta_q_d); - - franka::JointPositions output = add(q_start, delta_q_d); - output.motion_finished = motion_finished; - return output; - }); + calculateSynchronizedValues(delta_q, dq_max, ddq_max_start, ddq_max_goal, &dq_max_sync, &t_1_sync, &t_2_sync, + &t_f_sync, &q_1); + robot.control([=, &time](const franka::RobotState &, franka::Duration time_step) -> franka::JointPositions { + time += time_step.toSec(); + + std::array delta_q_d; + bool motion_finished = + calculateDesiredValues(time, delta_q, dq_max_sync, t_1_sync, t_2_sync, t_f_sync, q_1, &delta_q_d); + + franka::JointPositions output = add(q_start, delta_q_d); + output.motion_finished = motion_finished; + return output; + }); std::cout << std::endl << "Motion finished" << std::endl; } catch (const franka::Exception &e) { std::cout << e.what() << std::endl; @@ -149,12 +132,9 @@ int main(int argc, char **argv) return 0; } -bool calculateDesiredValues(double t, const std::array &delta_q, - const std::array &dq_max, - const std::array &t_1, - const std::array &t_2, - const std::array &t_f, - const std::array &q_1, +bool calculateDesiredValues(double t, const std::array &delta_q, const std::array &dq_max, + const std::array &t_1, const std::array &t_2, + const std::array &t_f, const std::array &q_1, std::array *delta_q_d) { std::array sign_delta_q; @@ -169,38 +149,29 @@ bool calculateDesiredValues(double t, const std::array &delta_q, joint_motion_finished[i] = true; } else { if (t < t_1[i]) { - (*delta_q_d)[i] = -1.0 / std::pow(t_1[i], 3) * dq_max[i] * - sign_delta_q[i] * (0.5 * t - t_1[i]) * - std::pow(t, 3); + (*delta_q_d)[i] = + -1.0 / std::pow(t_1[i], 3) * dq_max[i] * sign_delta_q[i] * (0.5 * t - t_1[i]) * std::pow(t, 3); } else if (t >= t_1[i] && t < t_2[i]) { (*delta_q_d)[i] = q_1[i] + (t - t_1[i]) * dq_max[i] * sign_delta_q[i]; } else if (t >= t_2[i] && t < t_f[i]) { - (*delta_q_d)[i] = - delta_q[i] + - 0.5 * - (1.0 / std::pow(delta_t_2[i], 3) * - (t - t_1[i] - 2 * delta_t_2[i] - t_d[i]) * - std::pow((t - t_1[i] - t_d[i]), 3) + - (2.0 * t - 2.0 * t_1[i] - delta_t_2[i] - 2.0 * t_d[i])) * - dq_max[i] * sign_delta_q[i]; + (*delta_q_d)[i] = delta_q[i] + 0.5 * + (1.0 / std::pow(delta_t_2[i], 3) * (t - t_1[i] - 2 * delta_t_2[i] - t_d[i]) * + std::pow((t - t_1[i] - t_d[i]), 3) + + (2.0 * t - 2.0 * t_1[i] - delta_t_2[i] - 2.0 * t_d[i])) * + dq_max[i] * sign_delta_q[i]; } else { (*delta_q_d)[i] = delta_q[i]; joint_motion_finished[i] = true; } } } - return std::all_of(joint_motion_finished.cbegin(), - joint_motion_finished.cend(), [](bool x) { return x; }); + return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(), [](bool x) { return x; }); } -void calculateSynchronizedValues(const std::array &delta_q, - const std::array &dq_max, - const std::array &ddq_max_start, - const std::array &ddq_max_goal, - std::array *dq_max_sync, - std::array *t_1_sync, - std::array *t_2_sync, - std::array *t_f_sync, +void calculateSynchronizedValues(const std::array &delta_q, const std::array &dq_max, + const std::array &ddq_max_start, const std::array &ddq_max_goal, + std::array *dq_max_sync, std::array *t_1_sync, + std::array *t_2_sync, std::array *t_f_sync, std::array *q_1) { std::array dq_max_reach = dq_max; @@ -212,17 +183,14 @@ void calculateSynchronizedValues(const std::array &delta_q, for (size_t i = 0; i < 7; i++) { sign_delta_q[i] = sgn(delta_q[i]); if (std::abs(delta_q[i]) > kDeltaQMotionFinished) { - if (std::abs(delta_q[i]) < - (3.0 / 4.0 * (std::pow(dq_max[i], 2) / ddq_max_start[i]) + - 3.0 / 4.0 * (std::pow(dq_max[i], 2) / ddq_max_goal[i]))) { - dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q[i] * sign_delta_q[i] * - (ddq_max_start[i] * ddq_max_goal[i]) / + if (std::abs(delta_q[i]) < (3.0 / 4.0 * (std::pow(dq_max[i], 2) / ddq_max_start[i]) + + 3.0 / 4.0 * (std::pow(dq_max[i], 2) / ddq_max_goal[i]))) { + dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q[i] * sign_delta_q[i] * (ddq_max_start[i] * ddq_max_goal[i]) / (ddq_max_start[i] + ddq_max_goal[i])); } t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start[i]; delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal[i]; - t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + - std::abs(delta_q[i]) / dq_max_reach[i]; + t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q[i]) / dq_max_reach[i]; } } @@ -236,19 +204,13 @@ void calculateSynchronizedValues(const std::array &delta_q, (*dq_max_sync)[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a); (*t_1_sync)[i] = 1.5 * (*dq_max_sync)[i] / ddq_max_start[i]; delta_t_2_sync[i] = 1.5 * (*dq_max_sync)[i] / ddq_max_goal[i]; - (*t_f_sync)[i] = (*t_1_sync)[i] / 2 + delta_t_2_sync[i] / 2 + - std::abs(delta_q[i] / (*dq_max_sync)[i]); + (*t_f_sync)[i] = (*t_1_sync)[i] / 2 + delta_t_2_sync[i] / 2 + std::abs(delta_q[i] / (*dq_max_sync)[i]); (*t_2_sync)[i] = (*t_f_sync)[i] - delta_t_2_sync[i]; - (*q_1)[i] = - (*dq_max_sync)[i] * sign_delta_q[i] * (0.5 * (*t_1_sync)[i]); + (*q_1)[i] = (*dq_max_sync)[i] * sign_delta_q[i] * (0.5 * (*t_1_sync)[i]); } } } #else -int main() -{ - std::cout << "This example needs libfranka to control Panda robot." - << std::endl; -} +int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; } #endif diff --git a/example/servo-franka/franka_motion_with_control.cpp b/example/servo-franka/franka_motion_with_control.cpp index fb4b4f9323..ed64fd2fe2 100644 --- a/example/servo-franka/franka_motion_with_control.cpp +++ b/example/servo-franka/franka_motion_with_control.cpp @@ -31,16 +31,13 @@ namespace class Controller { public: - Controller(size_t dq_filter_size, - const std::array &K_P, // NOLINT - const std::array &K_D) // NOLINT - : dq_current_filter_position_(0), dq_filter_size_(dq_filter_size), - K_P_(K_P), K_D_(K_D) + Controller(size_t dq_filter_size, const std::array &K_P, // NOLINT + const std::array &K_D) // NOLINT + : dq_current_filter_position_(0), dq_filter_size_(dq_filter_size), K_P_(K_P), K_D_(K_D) { std::fill(dq_d_.begin(), dq_d_.end(), 0); dq_buffer_.reset(new double[dq_filter_size_ * 7]); - std::fill(&dq_buffer_.get()[0], &dq_buffer_.get()[dq_filter_size_ * 7], - 0); + std::fill(&dq_buffer_.get()[0], &dq_buffer_.get()[dq_filter_size_ * 7], 0); } inline franka::Torques step(const franka::RobotState &state) @@ -49,8 +46,7 @@ class Controller std::array tau_J_d; // NOLINT for (size_t i = 0; i < 7; i++) { - tau_J_d[i] = K_P_[i] * (state.q_d[i] - state.q[i]) + - K_D_[i] * (dq_d_[i] - getDQFiltered(i)); + tau_J_d[i] = K_P_[i] * (state.q_d[i] - state.q[i]) + K_D_[i] * (dq_d_[i] - getDQFiltered(i)); } return tau_J_d; } @@ -60,8 +56,7 @@ class Controller for (size_t i = 0; i < 7; i++) { dq_buffer_.get()[dq_current_filter_position_ * 7 + i] = state.dq[i]; } - dq_current_filter_position_ = - (dq_current_filter_position_ + 1) % dq_filter_size_; + dq_current_filter_position_ = (dq_current_filter_position_ + 1) % dq_filter_size_; } double getDQFiltered(size_t index) const @@ -89,11 +84,9 @@ std::vector generateTrajectory(double a_max) // Generating a motion with smooth velocity and acceleration. // Squared sine is used for the acceleration/deceleration phase. std::vector trajectory; - constexpr double kTimeStep = 0.001; // [s] - constexpr double kAccelerationTime = - 1; // time spend accelerating and decelerating [s] - constexpr double kConstantVelocityTime = - 1; // time spend with constant speed [s] + constexpr double kTimeStep = 0.001; // [s] + constexpr double kAccelerationTime = 1; // time spend accelerating and decelerating [s] + constexpr double kConstantVelocityTime = 1; // time spend with constant speed [s] // obtained during the speed up // and slow down [rad/s^2] double a = 0; // [rad/s^2] @@ -106,8 +99,7 @@ std::vector generateTrajectory(double a_max) a = 0; } else { const double deceleration_time = - (kAccelerationTime + kConstantVelocityTime) - - t; // time spent in the deceleration phase + (kAccelerationTime + kConstantVelocityTime) - t; // time spent in the deceleration phase a = -pow(sin(deceleration_time * M_PI / kAccelerationTime), 2) * a_max; } v += a * kTimeStep; @@ -141,8 +133,7 @@ int main(int argc, char **argv) std::cout << "Initializing controller: " << std::endl; for (size_t i = 0; i < 7; i++) { - std::cout << i + 1 << ": K_P = " << K_P[i] << "\tK_D = " << K_D[i] - << std::endl; + std::cout << i + 1 << ": K_P = " << K_P[i] << "\tK_D = " << K_D[i] << std::endl; } std::cout << "dq filter size: " << filter_size << std::endl; Controller controller(filter_size, K_P, K_D); @@ -152,38 +143,33 @@ int main(int argc, char **argv) // Set additional parameters always before the control loop, NEVER in the // control loop! Set collision behavior. - robot.setCollisionBehavior({{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); + robot.setCollisionBehavior( + {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, + {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, + {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, + {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); size_t index = 0; int joint_number = std::stoi(argv[5]); std::vector trajectory = generateTrajectory(std::stod(argv[6])); - robot.control( - [&](const franka::RobotState &robot_state, franka::Duration) - -> franka::Torques { return controller.step(robot_state); }, - [&](const franka::RobotState &, - franka::Duration time_step) -> franka::JointVelocities { - index += time_step.toMSec(); - - if (index >= trajectory.size()) { - index = trajectory.size() - 1; - } - - franka::JointVelocities velocities{{0, 0, 0, 0, 0, 0, 0}}; - velocities.dq[joint_number] = trajectory[index]; - - if (index >= trajectory.size() - 1) { - return franka::MotionFinished(velocities); - } - return velocities; - }); + robot.control([&](const franka::RobotState &robot_state, + franka::Duration) -> franka::Torques { return controller.step(robot_state); }, + [&](const franka::RobotState &, franka::Duration time_step) -> franka::JointVelocities { + index += time_step.toMSec(); + + if (index >= trajectory.size()) { + index = trajectory.size() - 1; + } + + franka::JointVelocities velocities{{0, 0, 0, 0, 0, 0, 0}}; + velocities.dq[joint_number] = trajectory[index]; + + if (index >= trajectory.size() - 1) { + return franka::MotionFinished(velocities); + } + return velocities; + }); } catch (const franka::Exception &e) { std::cout << e.what() << std::endl; return -1; @@ -193,9 +179,5 @@ int main(int argc, char **argv) } #else -int main() -{ - std::cout << "This example needs libfranka to control Panda robot." - << std::endl; -} +int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; } #endif diff --git a/example/servo-franka/franka_print_joint_positions.cpp b/example/servo-franka/franka_print_joint_positions.cpp index 572cf4bf3e..491cfdc637 100644 --- a/example/servo-franka/franka_print_joint_positions.cpp +++ b/example/servo-franka/franka_print_joint_positions.cpp @@ -19,14 +19,11 @@ * https://frankaemika.github.io/docs for more details. */ -template -std::ostream &operator<<(std::ostream &ostream, const std::array &array) +template std::ostream &operator<<(std::ostream &ostream, const std::array &array) { ostream << "["; - std::copy(array.cbegin(), array.cend() - 1, - std::ostream_iterator(ostream, ",")); - std::copy(array.cend() - 1, array.cend(), - std::ostream_iterator(ostream)); + std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator(ostream, ",")); + std::copy(array.cend() - 1, array.cend(), std::ostream_iterator(ostream)); ostream << "]"; return ostream; } @@ -34,8 +31,7 @@ std::ostream &operator<<(std::ostream &ostream, const std::array &array) int main(int argc, char **argv) { if (argc != 2) { - std::cerr << "Usage: ./print_joint_positions " - << std::endl; + std::cerr << "Usage: ./print_joint_positions " << std::endl; return -1; } @@ -45,8 +41,7 @@ int main(int argc, char **argv) franka::RobotState state = robot.readOnce(); franka::Model model(robot.loadModel()); - for (franka::Frame frame = franka::Frame::kJoint1; - frame <= franka::Frame::kEndEffector; frame++) { + for (franka::Frame frame = franka::Frame::kJoint1; frame <= franka::Frame::kEndEffector; frame++) { std::cout << model.pose(frame, state) << std::endl; } } catch (franka::Exception const &e) { @@ -58,9 +53,5 @@ int main(int argc, char **argv) } #else -int main() -{ - std::cout << "This example needs libfranka to control Panda robot." - << std::endl; -} +int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; } #endif diff --git a/example/servo-pioneer/movePioneer.cpp b/example/servo-pioneer/movePioneer.cpp index 3f4835264a..d0a1b51776 100644 --- a/example/servo-pioneer/movePioneer.cpp +++ b/example/servo-pioneer/movePioneer.cpp @@ -68,13 +68,12 @@ int main() int main(int argc, char **argv) { try { - std::cout - << "\nWARNING: this program does no sensing or avoiding of " - "obstacles, \n" - "the robot WILL collide with any objects in the way! Make sure " - "the \n" - "robot has approximately 3 meters of free space on all sides.\n" - << std::endl; + std::cout << "\nWARNING: this program does no sensing or avoiding of " + "obstacles, \n" + "the robot WILL collide with any objects in the way! Make sure " + "the \n" + "robot has approximately 3 meters of free space on all sides.\n" + << std::endl; vpRobotPioneer robot; @@ -112,13 +111,9 @@ int main(int argc, char **argv) robot.setVelocity(vpRobot::REFERENCE_FRAME, v); v_mes = robot.getVelocity(vpRobot::REFERENCE_FRAME); - std::cout << "Trans. vel= " << v_mes[0] - << " m/s, Rot. vel=" << vpMath::deg(v_mes[1]) << " deg/s" - << std::endl; + std::cout << "Trans. vel= " << v_mes[0] << " m/s, Rot. vel=" << vpMath::deg(v_mes[1]) << " deg/s" << std::endl; v_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME); - std::cout << "Left wheel vel= " << v_mes[0] - << " m/s, Right wheel vel=" << v_mes[1] << " m/s" - << std::endl; + std::cout << "Left wheel vel= " << v_mes[0] << " m/s, Right wheel vel=" << v_mes[1] << " m/s" << std::endl; std::cout << "Battery=" << robot.getBatteryVoltage() << std::endl; vpTime::wait(t, 40); @@ -134,8 +129,7 @@ int main(int argc, char **argv) ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. " "Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV", - robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), - robot.getRotVel(), robot.getBatteryVoltage()); + robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage()); robot.unlock(); std::cout << "Ending robot thread..." << std::endl; diff --git a/example/servo-pioneer/servoPioneerPanSegment3D.cpp b/example/servo-pioneer/servoPioneerPanSegment3D.cpp index a481c39c3d..7df14ef380 100644 --- a/example/servo-pioneer/servoPioneerPanSegment3D.cpp +++ b/example/servo-pioneer/servoPioneerPanSegment3D.cpp @@ -83,8 +83,7 @@ #if defined(VISP_HAVE_PIONEER) && defined(VISP_HAVE_BICLOPS) int main(int argc, char **argv) { -#if defined(VISP_HAVE_DC1394) || 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 I; // Create a gray level image container @@ -93,11 +92,10 @@ int main(int argc, char **argv) // surface // double coef = 0.9/14.85; // At 0.9m, the blob has a surface of 14.85 // (Logitec sphere) - double coef = - 1.2 / 13.0; // At 1m, the blob has a surface of 11.3 (AVT Pike 032C) - double L = 0.21; // 3D horizontal segment length - double Z_d = 0.8; // Desired distance along Z between camera and segment - bool normalized = true; // segment normilized features are used + double coef = 1.2 / 13.0; // At 1m, the blob has a surface of 11.3 (AVT Pike 032C) + double L = 0.21; // 3D horizontal segment length + double Z_d = 0.8; // Desired distance along Z between camera and segment + bool normalized = true; // segment normilized features are used // Warning: To have a non singular task of rank 3, Y_d should be different // from 0 so that the optical axis doesn't intersect the horizontal @@ -105,8 +103,7 @@ int main(int argc, char **argv) double Y_d = -.11; // Desired distance along Y between camera and segment. vpColVector qm(2); // Measured head position qm = 0; - double qm_pan = - 0; // Measured pan position (tilt is not handled in that example) + double qm_pan = 0; // Measured pan position (tilt is not handled in that example) #ifdef USE_REAL_ROBOT // Initialize the biclops head @@ -217,10 +214,8 @@ int main(int argc, char **argv) for (int i = 0; i < 2; i++) { dot[i].setGraphics(true); dot[i].setComputeMoments(true); - dot[i].setEllipsoidShapePrecision( - 0.); // to track a blob without any constraint on the shape - dot[i].setGrayLevelPrecision( - 0.9); // to set the blob gray level bounds for binarisation + dot[i].setEllipsoidShapePrecision(0.); // to track a blob without any constraint on the shape + dot[i].setGrayLevelPrecision(0.9); // to set the blob gray level bounds for binarisation dot[i].setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad // inner and outside points // with bad gray level @@ -284,9 +279,7 @@ int main(int argc, char **argv) s_segment.setZ2(P[1].get_Z()); task.addFeature(s_segment, s_segment_d, - vpFeatureSegment::selectXc() | - vpFeatureSegment::selectL() | - vpFeatureSegment::selectAlpha()); + vpFeatureSegment::selectXc() | vpFeatureSegment::selectL() | vpFeatureSegment::selectAlpha()); #ifdef USE_PLOTTER // Create a window (500 by 500) at position (700, 10) with two graphics @@ -371,11 +364,10 @@ int main(int argc, char **argv) // task.getInteractionMatrix() * task.get_cVe() * task.get_eJe() // << std::endl; task.print() ; if (task.getTaskRank() != 3) - std::cout << "Warning: task is of rank " << task.getTaskRank() - << std::endl; + std::cout << "Warning: task is of rank " << task.getTaskRank() << std::endl; #ifdef USE_PLOTTER - graph.plot(0, iter, v); // plot velocities applied to the robot + graph.plot(0, iter, v); // plot velocities applied to the robot graph.plot(1, iter, task.getError()); // plot error vector #endif @@ -388,11 +380,9 @@ int main(int argc, char **argv) v_biclops[0] = v[2]; v_biclops[1] = 0; - std::cout << "Send velocity to the pionner: " << v_pioneer[0] - << " m/s " << vpMath::deg(v_pioneer[1]) << " deg/s" - << std::endl; - std::cout << "Send velocity to the biclops head: " - << vpMath::deg(v_biclops[0]) << " deg/s" << std::endl; + std::cout << "Send velocity to the pionner: " << v_pioneer[0] << " m/s " << vpMath::deg(v_pioneer[1]) + << " deg/s" << std::endl; + std::cout << "Send velocity to the biclops head: " << vpMath::deg(v_biclops[0]) << " deg/s" << std::endl; pioneer.setVelocity(vpRobot::REFERENCE_FRAME, v_pioneer); biclops.setVelocity(vpRobot::ARTICULAR_FRAME, v_biclops); @@ -400,8 +390,7 @@ int main(int argc, char **argv) // Draw a vertical line which corresponds to the desired x coordinate // of the dot cog - vpDisplay::displayLine(I, 0, cam.get_u0(), 479, cam.get_u0(), - vpColor::red); + vpDisplay::displayLine(I, 0, cam.get_u0(), 479, cam.get_u0(), vpColor::red); vpDisplay::flush(I); // A click in the viewer to exit diff --git a/example/servo-pioneer/servoPioneerPoint2DDepth.cpp b/example/servo-pioneer/servoPioneerPoint2DDepth.cpp index a5d937fc13..7bcfac8fec 100644 --- a/example/servo-pioneer/servoPioneerPoint2DDepth.cpp +++ b/example/servo-pioneer/servoPioneerPoint2DDepth.cpp @@ -57,8 +57,8 @@ #include #include -#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || \ - defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100) +#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 @@ -198,10 +198,8 @@ int main(int argc, char **argv) vpDot2 dot; dot.setGraphics(true); dot.setComputeMoments(true); - dot.setEllipsoidShapePrecision( - 0.); // to track a blob without any constraint on the shape - dot.setGrayLevelPrecision( - 0.9); // to set the blob gray level bounds for binarisation + dot.setEllipsoidShapePrecision(0.); // to track a blob without any constraint on the shape + dot.setGrayLevelPrecision(0.9); // to set the blob gray level bounds for binarisation dot.setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad inner // and outside points with bad // gray level @@ -291,8 +289,7 @@ int main(int argc, char **argv) // reference frame v = task.computeControlLaw(); - std::cout << "Send velocity to the pionner: " << v[0] << " m/s " - << vpMath::deg(v[1]) << " deg/s" << std::endl; + std::cout << "Send velocity to the pionner: " << v[0] << " m/s " << vpMath::deg(v[1]) << " deg/s" << std::endl; // Send the velocity to the robot robot.setVelocity(vpRobot::REFERENCE_FRAME, v); @@ -322,10 +319,5 @@ int main(int argc, char **argv) } } #else -int main() -{ - std::cout - << "You don't have the right 3rd party libraries to run this example..." - << std::endl; -} +int main() { std::cout << "You don't have the right 3rd party libraries to run this example..." << std::endl; } #endif diff --git a/example/servo-pioneer/servoPioneerPoint2DDepthWithoutVpServo.cpp b/example/servo-pioneer/servoPioneerPoint2DDepthWithoutVpServo.cpp index a793fa5a8a..0aecc1c09a 100644 --- a/example/servo-pioneer/servoPioneerPoint2DDepthWithoutVpServo.cpp +++ b/example/servo-pioneer/servoPioneerPoint2DDepthWithoutVpServo.cpp @@ -55,8 +55,8 @@ #include #include -#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || \ - defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100) +#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 @@ -196,10 +196,8 @@ int main(int argc, char **argv) vpDot2 dot; dot.setGraphics(true); dot.setComputeMoments(true); - dot.setEllipsoidShapePrecision( - 0.); // to track a blob without any constraint on the shape - dot.setGrayLevelPrecision( - 0.9); // to set the blob gray level bounds for binarisation + dot.setEllipsoidShapePrecision(0.); // to track a blob without any constraint on the shape + dot.setGrayLevelPrecision(0.9); // to set the blob gray level bounds for binarisation dot.setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad inner // and outside points with bad // gray level @@ -284,8 +282,7 @@ int main(int argc, char **argv) // reference frame v = -lambda * (L * cVe * eJe).pseudoInverse() * error; - std::cout << "Send velocity to the pionner: " << v[0] << " m/s " - << vpMath::deg(v[1]) << " deg/s" << std::endl; + std::cout << "Send velocity to the pionner: " << v[0] << " m/s " << vpMath::deg(v[1]) << " deg/s" << std::endl; // Send the velocity to the robot robot.setVelocity(vpRobot::REFERENCE_FRAME, v); @@ -311,10 +308,5 @@ int main(int argc, char **argv) } } #else -int main() -{ - std::cout - << "You don't have the right 3rd party libraries to run this example..." - << std::endl; -} +int main() { std::cout << "You don't have the right 3rd party libraries to run this example..." << std::endl; } #endif diff --git a/example/servo-pioneer/sonarPioneerReader.cpp b/example/servo-pioneer/sonarPioneerReader.cpp index 6e12e9b15b..c90649cfa4 100644 --- a/example/servo-pioneer/sonarPioneerReader.cpp +++ b/example/servo-pioneer/sonarPioneerReader.cpp @@ -112,17 +112,13 @@ void sonarPrinter(void) range = sonar.currentReadingPolar(start_angle, end_angle, &angle); printf(" front quadrant: %5.0f ", range); // if (range != sonar.getMaxRange()) - if (std::fabs(range - sonar.getMaxRange()) > - std::numeric_limits::epsilon()) + if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits::epsilon()) printf("%3.0f ", angle); printf("\n"); #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) // if (isInitialized && range != sonar.getMaxRange()) - if (isInitialized && std::fabs(range - sonar.getMaxRange()) > - std::numeric_limits::epsilon()) { - double x = - range * cos(vpMath::rad( - angle)); // position of the obstacle in the sensor frame + if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits::epsilon()) { + double x = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame double y = range * sin(vpMath::rad(angle)); // Conversion in pixels so that the robot frame is in the middle of the @@ -132,8 +128,7 @@ void sonarPrinter(void) vpDisplay::display(I); vpDisplay::displayLine(I, half_size, half_size, 0, 0, vpColor::red, 5); - vpDisplay::displayLine(I, half_size, half_size, 0, 2 * half_size - 1, - vpColor::red, 5); + vpDisplay::displayLine(I, half_size, half_size, 0, 2 * half_size - 1, vpColor::red, 5); vpDisplay::displayLine(I, half_size, half_size, i, j, vpColor::green, 3); vpDisplay::displayCross(I, i, j, 7, vpColor::blue); } @@ -142,24 +137,21 @@ void sonarPrinter(void) range = sonar.currentReadingPolar(-135, -45, &angle); printf(" right quadrant: %5.0f ", range); // if (range != sonar.getMaxRange()) - if (std::fabs(range - sonar.getMaxRange()) > - std::numeric_limits::epsilon()) + if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits::epsilon()) printf("%3.0f ", angle); printf("\n"); range = sonar.currentReadingPolar(45, 135, &angle); printf(" left quadrant: %5.0f ", range); // if (range != sonar.getMaxRange()) - if (std::fabs(range - sonar.getMaxRange()) > - std::numeric_limits::epsilon()) + if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits::epsilon()) printf("%3.0f ", angle); printf("\n"); range = sonar.currentReadingPolar(-135, 135, &angle); printf(" back quadrant: %5.0f ", range); // if (range != sonar.getMaxRange()) - if (std::fabs(range - sonar.getMaxRange()) > - std::numeric_limits::epsilon()) + if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits::epsilon()) printf("%3.0f ", angle); printf("\n"); @@ -172,12 +164,9 @@ void sonarPrinter(void) if (reading != NULL) { angle = reading->getSensorTh(); range = reading->getRange(); - double sx = - reading->getSensorX(); // position of the sensor in the robot frame + double sx = reading->getSensorX(); // position of the sensor in the robot frame double sy = reading->getSensorY(); - double ox = - range * cos(vpMath::rad( - angle)); // position of the obstacle in the sensor frame + double ox = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame double oy = range * sin(vpMath::rad(angle)); double x = sx + ox; // position of the obstacle in the robot frame double y = sy + oy; @@ -196,8 +185,7 @@ void sonarPrinter(void) #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) // if (isInitialized && range != sonar.getMaxRange()) - if (isInitialized && std::fabs(range - sonar.getMaxRange()) > - std::numeric_limits::epsilon()) { + if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits::epsilon()) { vpDisplay::displayLine(I, si, sj, i, j, vpColor::blue, 2); vpDisplay::displayCross(I, si, sj, 7, vpColor::blue); char legend[15]; @@ -277,13 +265,9 @@ int main(int argc, char **argv) double t = vpTime::measureTimeMs(); v_mes = robot->getVelocity(vpRobot::REFERENCE_FRAME); - std::cout << "Trans. vel= " << v_mes[0] - << " m/s, Rot. vel=" << vpMath::deg(v_mes[1]) << " deg/s" - << std::endl; + std::cout << "Trans. vel= " << v_mes[0] << " m/s, Rot. vel=" << vpMath::deg(v_mes[1]) << " deg/s" << std::endl; v_mes = robot->getVelocity(vpRobot::ARTICULAR_FRAME); - std::cout << "Left wheel vel= " << v_mes[0] - << " m/s, Right wheel vel=" << v_mes[1] << " m/s" - << std::endl; + std::cout << "Left wheel vel= " << v_mes[0] << " m/s, Right wheel vel=" << v_mes[1] << " m/s" << std::endl; std::cout << "Battery=" << robot->getBatteryVoltage() << std::endl; #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) @@ -294,8 +278,7 @@ int main(int argc, char **argv) { // Set the default output path std::string opath; -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX opath = "/tmp"; #elif defined(_WIN32) opath = "C:\\temp"; @@ -339,8 +322,8 @@ int main(int argc, char **argv) ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. " "Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV", - robot->getX(), robot->getY(), robot->getTh(), robot->getVel(), - robot->getRotVel(), robot->getBatteryVoltage()); + robot->getX(), robot->getY(), robot->getTh(), robot->getVel(), robot->getRotVel(), + robot->getBatteryVoltage()); robot->unlock(); std::cout << "Ending robot thread..." << std::endl; diff --git a/example/servo-ptu46/movePtu46.cpp b/example/servo-ptu46/movePtu46.cpp index b63a5cb3f9..1f5c811f55 100644 --- a/example/servo-ptu46/movePtu46.cpp +++ b/example/servo-ptu46/movePtu46.cpp @@ -52,8 +52,7 @@ */ #include #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #include #endif @@ -127,8 +126,7 @@ int main() #else int main() { - vpERROR_TRACE( - "You do not have a ptu-46 robot connected to your computer..."); + vpERROR_TRACE("You do not have a ptu-46 robot connected to your computer..."); return 0; } diff --git a/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp b/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp index 17685b9954..840e3139db 100644 --- a/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp +++ b/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp @@ -59,8 +59,7 @@ */ #include #include // Debug trace -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #include #endif #include @@ -108,16 +107,12 @@ void signalCtrC(int signumber) int main() { std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera frame" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl; 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; try { @@ -181,8 +176,7 @@ int main() vpTRACE("sets the current position of the visual feature "); vpFeaturePoint p; - vpFeatureBuilder::create( - p, cam, dot); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure p.set_Z(1); vpTRACE("sets the desired position of the visual feature "); @@ -230,8 +224,7 @@ int main() for (;;) #endif { - std::cout << "---------------------------------------------" << iter - << std::endl; + std::cout << "---------------------------------------------" << iter << std::endl; g.acquire(I); vpDisplay::display(I); @@ -272,7 +265,6 @@ int main() #else int main() { - vpERROR_TRACE("You don't have a ptu-46 head connected to your computer ", - "or 1394 framegrabbing capabilities..."); + vpERROR_TRACE("You don't have a ptu-46 head connected to your computer ", "or 1394 framegrabbing capabilities..."); } #endif diff --git a/example/servo-viper650/servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp b/example/servo-viper650/servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp index bff1ad4513..b76f610381 100644 --- a/example/servo-viper650/servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp +++ b/example/servo-viper650/servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp @@ -61,8 +61,7 @@ #include -#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_DC1394) && \ - defined(VISP_HAVE_X11) +#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_X11) #include #include @@ -97,8 +96,8 @@ Lagrange or Dementhon methods. */ -void compute_pose(std::vector &point, std::vector &dot, - vpCameraParameters cam, vpHomogeneousMatrix &cMo, bool init) +void compute_pose(std::vector &point, std::vector &dot, vpCameraParameters cam, + vpHomogeneousMatrix &cMo, bool init) { vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon method vpHomogeneousMatrix cMo_lagrange; // computed pose with lagrange method @@ -110,7 +109,7 @@ void compute_pose(std::vector &point, std::vector &dot, vpImagePoint cog = dot[i].getCog(); vpPixelMeterConversion::convertPoint(cam, cog, x, y); // pixel to meter conversion - point[i].set_x(x); // projection perspective p + point[i].set_x(x); // projection perspective p point[i].set_y(y); pose.addPoint(point[i]); } @@ -169,8 +168,7 @@ int main() vpRobotViper650 robot; // Load the end-effector to camera frame transformation obtained // using a camera intrinsic model with distortion - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithDistortion; + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithDistortion; robot.init(vpRobotViper650::TOOL_PTGREY_FLEA2_CAMERA, projModel); vpHomogeneousMatrix eMc; robot.get_eMc(eMc); @@ -196,9 +194,7 @@ int main() vpImagePoint cog; - std::cout - << "Click on the 4 dots clockwise starting from upper/left dot..." - << std::endl; + std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl; for (size_t i = 0; i < dot.size(); i++) { dot[i].setGraphics(true); @@ -217,8 +213,7 @@ int main() // Sets the current position of the visual feature vpFeaturePoint p[4]; for (size_t i = 0; i < dot.size(); i++) - vpFeatureBuilder::create( - p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure + vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure // Set the position of the square target in a frame which origin is // centered in the middle of the square @@ -234,9 +229,8 @@ int main() std::cout << "Initial camera pose (cMo): \n" << cMo << std::endl; // Initialise a desired pose to compute s*, the desired 2D point features - vpHomogeneousMatrix cMo_d( - vpTranslationVector(0, 0, 0.5), // tz = 0.5 meter - vpRotationMatrix()); // no rotation + vpHomogeneousMatrix cMo_d(vpTranslationVector(0, 0, 0.5), // tz = 0.5 meter + vpRotationMatrix()); // no rotation // Sets the desired position of the 2D visual feature vpFeaturePoint pd[4]; @@ -277,8 +271,7 @@ int main() // Initialise the velocity control of the robot robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); - std::cout << "\nHit CTRL-C or click in the image to stop the loop...\n" - << std::flush; + std::cout << "\nHit CTRL-C or click in the image to stop the loop...\n" << std::flush; for (;;) { // Acquire a new image from the camera g.acquire(I); @@ -297,8 +290,7 @@ int main() vpDisplay::displayCross(I, cog, 10, vpColor::green); } } catch (...) { - std::cout << "Error detected while tracking visual features.." - << std::endl; + std::cout << "Error detected while tracking visual features.." << std::endl; break; } @@ -336,8 +328,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to joint translation velocities in m/s // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -347,8 +338,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -358,8 +348,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the 4 feature points. For each feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent-SR300.cpp b/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent-SR300.cpp index 1679dd3ff4..6deedb0939 100644 --- a/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent-SR300.cpp +++ b/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent-SR300.cpp @@ -64,8 +64,7 @@ #include -#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_REALSENSE) && \ - defined(VISP_HAVE_X11) +#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_X11) #include #include @@ -100,8 +99,8 @@ Lagrange or Dementhon methods. */ -void compute_pose(std::vector &point, std::vector &dot, - vpCameraParameters cam, vpHomogeneousMatrix &cMo, bool init) +void compute_pose(std::vector &point, std::vector &dot, vpCameraParameters cam, + vpHomogeneousMatrix &cMo, bool init) { vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon method vpHomogeneousMatrix cMo_lagrange; // computed pose with lagrange method @@ -113,7 +112,7 @@ void compute_pose(std::vector &point, std::vector &dot, vpImagePoint cog = dot[i].getCog(); vpPixelMeterConversion::convertPoint(cam, cog, x, y); // pixel to meter conversion - point[i].set_x(x); // projection perspective p + point[i].set_x(x); // projection perspective p point[i].set_y(y); pose.addPoint(point[i]); } @@ -189,14 +188,12 @@ int main() g.setEnableStream(rs::stream::depth, false); g.setEnableStream(rs::stream::infrared, false); g.setEnableStream(rs::stream::infrared2, false); - g.setStreamSettings( - rs::stream::color, - vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30)); + g.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30)); g.open(); // Update camera parameters - vpCameraParameters cam = g.getCameraParameters( - rs::stream::color, vpCameraParameters::perspectiveProjWithDistortion); + vpCameraParameters cam = + g.getCameraParameters(rs::stream::color, vpCameraParameters::perspectiveProjWithDistortion); std::cout << "Camera intrinsic parameters: \n" << cam << std::endl; g.acquire(I); @@ -207,9 +204,7 @@ int main() std::vector dot(4); - std::cout - << "Click on the 4 dots clockwise starting from upper/left dot..." - << std::endl; + std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl; for (size_t i = 0; i < dot.size(); i++) { dot[i].setGraphics(true); @@ -222,8 +217,7 @@ int main() // Sets the current position of the visual feature vpFeaturePoint p[4]; for (size_t i = 0; i < dot.size(); i++) - vpFeatureBuilder::create( - p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure + vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure // Set the position of the square target in a frame which origin is // centered in the middle of the square @@ -239,9 +233,8 @@ int main() std::cout << "Initial camera pose (cMo): \n" << cMo << std::endl; // Initialise a desired pose to compute s*, the desired 2D point features - vpHomogeneousMatrix cMo_d( - vpTranslationVector(0, 0, 0.5), // tz = 0.5 meter - vpRotationMatrix()); // no rotation + vpHomogeneousMatrix cMo_d(vpTranslationVector(0, 0, 0.5), // tz = 0.5 meter + vpRotationMatrix()); // no rotation // Sets the desired position of the 2D visual feature vpFeaturePoint pd[4]; @@ -273,8 +266,7 @@ int main() // Initialise the velocity control of the robot robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); - std::cout << "\nHit CTRL-C or click in the image to stop the loop...\n" - << std::flush; + std::cout << "\nHit CTRL-C or click in the image to stop the loop...\n" << std::flush; for (;;) { // Acquire a new image from the camera g.acquire(I); @@ -293,8 +285,7 @@ int main() vpDisplay::displayCross(I, cog, 10, vpColor::green); } } catch (...) { - std::cout << "Error detected while tracking visual features.." - << std::endl; + std::cout << "Error detected while tracking visual features.." << std::endl; break; } @@ -325,8 +316,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to camera translation velocities in m/s // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -336,8 +326,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -347,8 +336,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the 4 feature points. For each feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp b/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp index 20d95c393f..d9f566b9fa 100644 --- a/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp +++ b/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp @@ -62,8 +62,7 @@ #include -#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_DC1394) && \ - defined(VISP_HAVE_X11) +#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_X11) #include #include @@ -98,8 +97,8 @@ Lagrange or Dementhon methods. */ -void compute_pose(std::vector &point, std::vector &dot, - vpCameraParameters cam, vpHomogeneousMatrix &cMo, bool init) +void compute_pose(std::vector &point, std::vector &dot, vpCameraParameters cam, + vpHomogeneousMatrix &cMo, bool init) { vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon method vpHomogeneousMatrix cMo_lagrange; // computed pose with lagrange method @@ -111,7 +110,7 @@ void compute_pose(std::vector &point, std::vector &dot, vpImagePoint cog = dot[i].getCog(); vpPixelMeterConversion::convertPoint(cam, cog, x, y); // pixel to meter conversion - point[i].set_x(x); // projection perspective p + point[i].set_x(x); // projection perspective p point[i].set_y(y); pose.addPoint(point[i]); } @@ -170,8 +169,7 @@ int main() vpRobotViper650 robot; // Load the end-effector to camera frame transformation obtained // using a camera intrinsic model with distortion - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithDistortion; + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithDistortion; robot.init(vpRobotViper650::TOOL_PTGREY_FLEA2_CAMERA, projModel); vpHomogeneousMatrix eMc; robot.get_eMc(eMc); @@ -195,9 +193,7 @@ int main() std::vector dot(4); - std::cout - << "Click on the 4 dots clockwise starting from upper/left dot..." - << std::endl; + std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl; for (size_t i = 0; i < dot.size(); i++) { dot[i].setGraphics(true); @@ -216,8 +212,7 @@ int main() // Sets the current position of the visual feature vpFeaturePoint p[4]; for (size_t i = 0; i < dot.size(); i++) - vpFeatureBuilder::create( - p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure + vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure // Set the position of the square target in a frame which origin is // centered in the middle of the square @@ -233,9 +228,8 @@ int main() std::cout << "Initial camera pose (cMo): \n" << cMo << std::endl; // Initialise a desired pose to compute s*, the desired 2D point features - vpHomogeneousMatrix cMo_d( - vpTranslationVector(0, 0, 0.5), // tz = 0.5 meter - vpRotationMatrix()); // no rotation + vpHomogeneousMatrix cMo_d(vpTranslationVector(0, 0, 0.5), // tz = 0.5 meter + vpRotationMatrix()); // no rotation // Sets the desired position of the 2D visual feature vpFeaturePoint pd[4]; @@ -267,8 +261,7 @@ int main() // Initialise the velocity control of the robot robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); - std::cout << "\nHit CTRL-C or click in the image to stop the loop...\n" - << std::flush; + std::cout << "\nHit CTRL-C or click in the image to stop the loop...\n" << std::flush; for (;;) { // Acquire a new image from the camera g.acquire(I); @@ -287,8 +280,7 @@ int main() vpDisplay::displayCross(I, cog, 10, vpColor::green); } } catch (...) { - std::cout << "Error detected while tracking visual features.." - << std::endl; + std::cout << "Error detected while tracking visual features.." << std::endl; break; } @@ -319,8 +311,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to camera translation velocities in m/s // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -330,8 +321,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -341,8 +331,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the 4 feature points. For each feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-viper650/servoViper650Point2DCamVelocity.cpp b/example/servo-viper650/servoViper650Point2DCamVelocity.cpp index 77104cdb0c..7fb569df46 100644 --- a/example/servo-viper650/servoViper650Point2DCamVelocity.cpp +++ b/example/servo-viper650/servoViper650Point2DCamVelocity.cpp @@ -62,8 +62,7 @@ #include -#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_DC1394) && \ - defined(VISP_HAVE_X11) +#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_X11) #include #include @@ -178,8 +177,7 @@ int main() // Now the robot will be controlled in velocity robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL); - std::cout << "\nHit CTRL-C or click in the image to stop the loop...\n" - << std::flush; + std::cout << "\nHit CTRL-C or click in the image to stop the loop...\n" << std::flush; for (;;) { // Acquire a new image from the camera @@ -192,8 +190,7 @@ int main() // Achieve the tracking of the dot in the image dot.track(I); } catch (...) { - std::cout << "Error detected while tracking visual features.." - << std::endl; + std::cout << "Error detected while tracking visual features.." << std::endl; break; } @@ -218,8 +215,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to camera translation velocities in m/s // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -229,8 +225,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -240,8 +235,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the feature point. For this feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp b/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp index 8d482a01b5..28e0c547e6 100644 --- a/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp +++ b/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp @@ -103,8 +103,7 @@ Lagrange or Dementhon methods. */ -void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, - vpCameraParameters cam, vpHomogeneousMatrix &cMo, +void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, vpCameraParameters cam, vpHomogeneousMatrix &cMo, vpTranslationVector &cto, vpRxyzVector &cro, bool init) { vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon @@ -118,7 +117,7 @@ void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, cog = dot[i].getCog(); vpPixelMeterConversion::convertPoint(cam, cog, x, y); // pixel to meter conversion - point[i].set_x(x); // projection perspective p + point[i].set_x(x); // projection perspective p point[i].set_y(y); pose.addPoint(point[i]); } @@ -184,8 +183,7 @@ int main() vpRobotViper850 robot; // Load the end-effector to camera frame transformation obtained // using a camera intrinsic model with distortion - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithDistortion; + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithDistortion; robot.init(vpRobotViper850::TOOL_PTGREY_FLEA2_CAMERA, projModel); vpServo task; @@ -213,25 +211,18 @@ int main() vpDisplay::flush(I); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the joint space" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the joint space" << std::endl; std::cout << " Use of the Afma6 robot " << std::endl; - std::cout << " task : servo 4 points on a square with dimention " << L - << " meters" << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; vpDot2 dot[4]; vpImagePoint cog; - std::cout - << "Click on the 4 dots clockwise starting from upper/left dot..." - << std::endl; + std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl; for (i = 0; i < 4; i++) { dot[i].setGraphics(true); @@ -251,8 +242,7 @@ int main() // Sets the current position of the visual feature vpFeaturePoint p[4]; for (i = 0; i < 4; i++) - vpFeatureBuilder::create( - p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure + vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure // Set the position of the square target in a frame which origin is // centered in the middle of the square @@ -373,8 +363,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to joint translation velocities in m/s // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -384,8 +373,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -395,8 +383,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the 4 feature points. For each feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionDesired.cpp b/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionDesired.cpp index 43c94475c8..b5d4df89c0 100644 --- a/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionDesired.cpp +++ b/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionDesired.cpp @@ -121,8 +121,7 @@ int main() vpRobotViper850 robot; // Load the end-effector to camera frame transformation obtained // using a camera intrinsic model with distortion - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithDistortion; + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithDistortion; robot.init(vpRobotViper850::TOOL_PTGREY_FLEA2_CAMERA, projModel); vpServo task; @@ -150,25 +149,18 @@ int main() vpDisplay::flush(I); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the joint space" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the joint space" << std::endl; std::cout << " Use of the Afma6 robot " << std::endl; - std::cout << " task : servo 4 points on a square with dimention " << L - << " meters" << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; vpDot dot[4]; vpImagePoint cog; - std::cout - << "Click on the 4 dots clockwise starting from upper/left dot..." - << std::endl; + std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl; for (i = 0; i < 4; i++) { dot[i].setGraphics(true); @@ -188,8 +180,7 @@ int main() // Sets the current position of the visual feature vpFeaturePoint p[4]; for (i = 0; i < 4; i++) - vpFeatureBuilder::create( - p[i], cam, dot[i]); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y and Z of the vpPoint structure // sets the desired position of the visual feature vpFeaturePoint pd[4]; @@ -280,8 +271,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to joint translation velocities in m/s // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -291,8 +281,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -302,8 +291,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the 4 feature points. For each feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-viper850/servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp b/example/servo-viper850/servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp index 24e39eec7a..82afa049c5 100644 --- a/example/servo-viper850/servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp +++ b/example/servo-viper850/servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp @@ -104,8 +104,7 @@ Lagrange or Dementhon methods. */ -void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, - vpCameraParameters cam, vpHomogeneousMatrix &cMo, +void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, vpCameraParameters cam, vpHomogeneousMatrix &cMo, vpTranslationVector &cto, vpRxyzVector &cro, bool init) { vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon @@ -119,7 +118,7 @@ void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, cog = dot[i].getCog(); vpPixelMeterConversion::convertPoint(cam, cog, x, y); // pixel to meter conversion - point[i].set_x(x); // projection perspective p + point[i].set_x(x); // projection perspective p point[i].set_y(y); pose.addPoint(point[i]); } @@ -185,8 +184,7 @@ int main() vpRobotViper850 robot; // Load the end-effector to camera frame transformation obtained // using a camera intrinsic model with distortion - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithDistortion; + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithDistortion; robot.init(vpRobotViper850::TOOL_PTGREY_FLEA2_CAMERA, projModel); vpServo task; @@ -214,25 +212,18 @@ int main() vpDisplay::flush(I); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera space" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera space" << std::endl; std::cout << " Use of the Viper850 robot " << std::endl; - std::cout << " task : servo 4 points on a square with dimention " << L - << " meters" << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; vpDot2 dot[4]; vpImagePoint cog; - std::cout - << "Click on the 4 dots clockwise starting from upper/left dot..." - << std::endl; + std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl; for (i = 0; i < 4; i++) { dot[i].setGraphics(true); @@ -252,8 +243,7 @@ int main() // Sets the current position of the visual feature vpFeaturePoint p[4]; for (i = 0; i < 4; i++) - vpFeatureBuilder::create( - p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure + vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure // Set the position of the square target in a frame which origin is // centered in the middle of the square @@ -356,8 +346,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to joint translation velocities in m/s // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -367,8 +356,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -378,8 +366,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the 4 feature points. For each feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-viper850/servoViper850FourPointsKinect.cpp b/example/servo-viper850/servoViper850FourPointsKinect.cpp index 56f73e18d1..901334982d 100644 --- a/example/servo-viper850/servoViper850FourPointsKinect.cpp +++ b/example/servo-viper850/servoViper850FourPointsKinect.cpp @@ -56,8 +56,7 @@ #include #include -#if (defined(VISP_HAVE_VIPER850) && \ - defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES)) +#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES)) #include #include @@ -109,8 +108,7 @@ Lagrange or Dementhon methods. */ -void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, - vpCameraParameters cam, vpHomogeneousMatrix &cMo, +void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, vpCameraParameters cam, vpHomogeneousMatrix &cMo, vpTranslationVector &cto, vpRxyzVector &cro, bool init) { vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon @@ -124,7 +122,7 @@ void compute_pose(vpPoint point[], vpDot2 dot[], int ndot, cog = dot[i].getCog(); vpPixelMeterConversion::convertPoint(cam, cog, x, y); // pixel to meter conversion - point[i].set_x(x); // projection perspective p + point[i].set_x(x); // projection perspective p point[i].set_y(y); pose.addPoint(point[i]); } @@ -190,8 +188,7 @@ int main() vpRobotViper850 robot; // Load the end-effector to camera frame transformation obtained // using a camera intrinsic model with distortion - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithDistortion; + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithDistortion; robot.init(vpRobotViper850::TOOL_GENERIC_CAMERA, projModel); vpServo task; @@ -226,25 +223,18 @@ int main() vpDisplay::flush(I); std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the camera space" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the camera space" << std::endl; std::cout << " Use of the Viper850 robot " << std::endl; - std::cout << " task : servo 4 points on a square with dimention " << L - << " meters" << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; vpDot2 dot[4]; vpImagePoint cog; - std::cout - << "Click on the 4 dots clockwise starting from upper/left dot..." - << std::endl; + std::cout << "Click on the 4 dots clockwise starting from upper/left dot..." << std::endl; for (i = 0; i < 4; i++) { dot[i].initTracking(I); @@ -264,8 +254,7 @@ int main() // Sets the current position of the visual feature vpFeaturePoint p[4]; for (i = 0; i < 4; i++) - vpFeatureBuilder::create( - p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure + vpFeatureBuilder::create(p[i], cam, dot[i]); // retrieve x,y of the vpFeaturePoint structure // Set the position of the square target in a frame which origin is // centered in the middle of the square @@ -370,8 +359,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to joint translation velocities in m/s // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -381,8 +369,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -392,8 +379,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the 4 feature points. For each feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp index 09bdf5ae08..3122dce2a3 100644 --- a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp +++ b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp @@ -59,8 +59,7 @@ #include #include -#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394) && \ - defined(VISP_HAVE_DISPLAY)) +#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_DISPLAY)) #include #include @@ -219,8 +218,7 @@ int main() // sets the current position of the visual feature vpFeaturePoint p; - vpFeatureBuilder::create( - p, cam, dot); // retrieve x,y and Z of the vpPoint structure + 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 @@ -345,8 +343,7 @@ int main() for (unsigned int j = 0; j < 6; j++) // j is the joint // if (pb[j]==1) { - if (std::fabs(pb[j] - 1) <= - std::numeric_limits::epsilon()) { + if (std::fabs(pb[j] - 1) <= std::numeric_limits::epsilon()) { for (unsigned int i = 0; i < dimKernelL; i++) E[k][i] = kernelJ1[j][i]; @@ -384,15 +381,13 @@ int main() data[i] *= 2; } unsigned int joint = 2; - data[6] = - 2 * (tQmin[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]); - data[7] = - 2 * (tQmax[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]); + data[6] = 2 * (tQmin[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]); + data[7] = 2 * (tQmax[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]); data[8] = -1; data[9] = 1; plot.plot(0, iter, data); // plot q, Qmin, Qmax, tQmin, tQmax - plot.plot(1, iter, v); // plot joint velocities applied to the robot + plot.plot(1, iter, v); // plot joint velocities applied to the robot } vpDisplay::flush(I); diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp index 457cda63b3..775a6f473e 100644 --- a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp +++ b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp @@ -56,8 +56,7 @@ #include #include -#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394) && \ - defined(VISP_HAVE_DISPLAY)) +#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_DISPLAY)) #include #include @@ -199,8 +198,7 @@ int main() // sets the current position of the visual feature vpFeaturePoint p; - vpFeatureBuilder::create( - p, cam, dot); // retrieve x,y and Z of the vpPoint structure + 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 @@ -315,10 +313,8 @@ int main() data[i] *= 2; } unsigned int joint = 2; - data[6] = - 2 * (tQmin[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]); - data[7] = - 2 * (tQmax[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]); + data[6] = 2 * (tQmin[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]); + data[7] = 2 * (tQmax[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]); data[8] = -1; data[9] = 1; plot.plot(0, iter, data); // plot q, Qmin, Qmax, tQmin, tQmax diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-large.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-large.cpp index f534430478..b5cc6f46c7 100644 --- a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-large.cpp +++ b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-large.cpp @@ -57,8 +57,7 @@ #include #include -#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394_2) && \ - defined(VISP_HAVE_DISPLAY)) +#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394_2) && defined(VISP_HAVE_DISPLAY)) #include #include @@ -182,8 +181,7 @@ int main() // sets the current position of the visual feature vpFeaturePoint p; - vpFeatureBuilder::create( - p, cam, dot); // retrieve x,y and Z of the vpPoint structure + 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 @@ -253,8 +251,7 @@ int main() prim_task = task.computeControlLaw(); // Compute the secondary task for the joint limit avoidance - sec_task = task.secondaryTaskJointLimitAvoidance(q, prim_task, jointMin, - jointMax, rho, rho1); + sec_task = task.secondaryTaskJointLimitAvoidance(q, prim_task, jointMin, jointMax, rho, rho1); vpColVector v; v = prim_task + sec_task; @@ -279,24 +276,16 @@ int main() 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]); + 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 diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity.cpp index d9ce9ce09b..1749ffe48b 100644 --- a/example/servo-viper850/servoViper850Point2DArtVelocity.cpp +++ b/example/servo-viper850/servoViper850Point2DArtVelocity.cpp @@ -141,16 +141,12 @@ int main() // exit(1) ; std::cout << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " Test program for vpServo " << std::endl; - std::cout - << " Eye-in-hand task control, velocity computed in the joint space" - << std::endl; + std::cout << " Eye-in-hand task control, velocity computed in the joint space" << std::endl; std::cout << " Use of the Afma6 robot " << std::endl; std::cout << " task : servo a point " << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; vpDot2 dot; @@ -167,8 +163,7 @@ int main() vpTRACE("sets the current position of the visual feature "); vpFeaturePoint p; - vpFeatureBuilder::create( - p, cam, dot); // retrieve x,y and Z of the vpPoint structure + vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure p.set_Z(1); vpTRACE("sets the desired position of the visual feature "); @@ -248,8 +243,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to joint translation velocities in m/s // v[3], v[4], v[5] correspond to joint rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -259,8 +253,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -270,8 +263,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the feature point. For this feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-viper850/servoViper850Point2DCamVelocity.cpp b/example/servo-viper850/servoViper850Point2DCamVelocity.cpp index 2d60ed78c9..26ae02870b 100644 --- a/example/servo-viper850/servoViper850Point2DCamVelocity.cpp +++ b/example/servo-viper850/servoViper850Point2DCamVelocity.cpp @@ -132,14 +132,11 @@ int main() g.open(I); #ifdef VISP_HAVE_X11 - vpDisplayX display(I, (int)(100 + I.getWidth() + 30), 200, - "Current image"); + vpDisplayX display(I, (int)(100 + I.getWidth() + 30), 200, "Current image"); #elif defined(VISP_HAVE_OPENCV) - vpDisplayOpenCV display(I, (int)(100 + I.getWidth() + 30), 200, - "Current image"); + vpDisplayOpenCV display(I, (int)(100 + I.getWidth() + 30), 200, "Current image"); #elif defined(VISP_HAVE_GTK) - vpDisplayGTK display(I, (int)(100 + I.getWidth() + 30), 200, - "Current image"); + vpDisplayGTK display(I, (int)(100 + I.getWidth() + 30), 200, "Current image"); #endif vpDisplay::display(I); @@ -232,8 +229,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to camera translation velocities in m/s // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -243,8 +239,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -254,8 +249,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the feature point. For this feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/servo-viper850/servoViper850Point2DCamVelocityKalman.cpp b/example/servo-viper850/servoViper850Point2DCamVelocityKalman.cpp index 11202b0e22..0a421ff3ba 100644 --- a/example/servo-viper850/servoViper850Point2DCamVelocityKalman.cpp +++ b/example/servo-viper850/servoViper850Point2DCamVelocityKalman.cpp @@ -128,13 +128,12 @@ int main() vpColVector sigma_measure(nsignal); unsigned int state_size = 0; // Kalman state vector size - kalman.setStateModel(vpLinearKalmanFilterInstantiation:: - stateConstVelWithColoredNoise_MeasureVel); + kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel); state_size = kalman.getStateSize(); sigma_state.resize(state_size * nsignal); sigma_state = 0.00001; // Same state variance for all signals sigma_measure = 0.05; // Same measure variance for all the signals - double dummy = 0; // non used parameter dt for the velocity state model + double dummy = 0; // non used parameter dt for the velocity state model kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy); // Initialize the robot @@ -176,14 +175,11 @@ int main() } #ifdef VISP_HAVE_X11 - vpDisplayX display(I, (int)(100 + I.getWidth() + 30), 200, - "Current image"); + vpDisplayX display(I, (int)(100 + I.getWidth() + 30), 200, "Current image"); #elif defined(VISP_HAVE_OPENCV) - vpDisplayOpenCV display(I, (int)(100 + I.getWidth() + 30), 200, - "Current image"); + vpDisplayOpenCV display(I, (int)(100 + I.getWidth() + 30), 200, "Current image"); #elif defined(VISP_HAVE_GTK) - vpDisplayGTK display(I, (int)(100 + I.getWidth() + 30), 200, - "Current image"); + vpDisplayGTK display(I, (int)(100 + I.getWidth() + 30), 200, "Current image"); #endif vpDisplay::display(I); @@ -336,8 +332,7 @@ int main() // Save velocities applied to the robot in the log file // v[0], v[1], v[2] correspond to camera translation velocities in m/s // v[3], v[4], v[5] correspond to camera rotation velocities in rad/s - flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] - << " " << v[5] << " "; + flog << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5] << " "; // Get the measured joint velocities of the robot vpColVector qvel; @@ -347,8 +342,7 @@ int main() // velocities in m/s // - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation // velocities in rad/s - flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] - << " " << qvel[4] << " " << qvel[5] << " "; + flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " "; // Get the measured joint positions of the robot vpColVector q; @@ -358,8 +352,7 @@ int main() // positions in m // - q[3], q[4], q[5] correspond to measured joint rotation // positions in rad - flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] - << " " << q[5] << " "; + flog << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << q[4] << " " << q[5] << " "; // Save feature error (s-s*) for the feature point. For this feature // point, we have 2 errors (along x and y axis). This error is diff --git a/example/tools/histogram.cpp b/example/tools/histogram.cpp index 44e6bb1e64..a1d868c0dc 100644 --- a/example/tools/histogram.cpp +++ b/example/tools/histogram.cpp @@ -75,8 +75,7 @@ \param user : Username. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string opath, std::string user) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user) { fprintf(stdout, "\n\ Read an image on the disk, display it using X11, display some\n\ @@ -124,8 +123,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, const std::string &user) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, const std::string &user) { const char *optarg_; int c; @@ -224,8 +222,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -234,13 +231,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, opath, username); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -282,8 +276,7 @@ int main(int argc, const char **argv) vpTRACE("List of peaks"); vpTRACE("Nb peaks: %d", nbpeaks); if (nbpeaks) { - for (std::list::const_iterator it = peaks.begin(); - it != peaks.end(); ++it) { + for (std::list::const_iterator it = peaks.begin(); it != peaks.end(); ++it) { vpHistogramPeak p = *it; vpTRACE("Peak: gray level: %d value: %d", p.getLevel(), p.getValue()); } @@ -296,8 +289,7 @@ int main(int argc, const char **argv) vpTRACE("Sorted list of peaks"); vpTRACE("Nb peaks: %d", nbpeaks); if (nbpeaks) { - for (std::list::const_iterator it = peaks.begin(); - it != peaks.end(); ++it) { + for (std::list::const_iterator it = peaks.begin(); it != peaks.end(); ++it) { vpHistogramPeak p = *it; vpTRACE("Peak: gray level: %d value: %d", p.getLevel(), p.getValue()); } @@ -309,9 +301,8 @@ int main(int argc, const char **argv) if (nbpeaks != 2) { std::cout << "Not a bimodal histogram..." << std::endl; } else { - vpTRACE("Bimodal histogram: main peak1: %d-%d second peak2: %d-%d", - peak1.getLevel(), peak1.getValue(), peak2.getLevel(), - peak2.getValue()); + vpTRACE("Bimodal histogram: main peak1: %d-%d second peak2: %d-%d", peak1.getLevel(), peak1.getValue(), + peak2.getLevel(), peak2.getValue()); } // Get the valey between the two highest peaks @@ -328,15 +319,12 @@ int main(int argc, const char **argv) // Search the two valeys around peak1 unsigned ret = h.getValey(distance, peak1, valeyl, valeyr); if (ret == 0x00) { - vpTRACE("No left and right valey for peak %d-%d...", peak1.getLevel(), - peak1.getValue()); + vpTRACE("No left and right valey for peak %d-%d...", peak1.getLevel(), peak1.getValue()); } else if (ret == 0x10) { - vpTRACE("No right valey for peak %d-%d...", peak1.getLevel(), - peak1.getValue()); + vpTRACE("No right valey for peak %d-%d...", peak1.getLevel(), peak1.getValue()); vpTRACE("Left valey: %d-%d", valeyl.getLevel(), valeyl.getValue()); } else if (ret == 0x01) { - vpTRACE("No left valey for peak %d-%d...", peak1.getLevel(), - peak1.getValue()); + vpTRACE("No left valey for peak %d-%d...", peak1.getLevel(), peak1.getValue()); vpTRACE("Right valey: %d-%d", valeyr.getLevel(), valeyr.getValue()); } else if (ret == 0x11) { vpTRACE("Left valey: %d-%d", valeyl.getLevel(), valeyl.getValue()); @@ -347,15 +335,12 @@ int main(int argc, const char **argv) // Search the two valeys around peak2 unsigned ret = h.getValey(distance, peak2, valeyl, valeyr); if (ret == 0x00) { - vpTRACE("No left and right valey for peak %d-%d...", peak2.getLevel(), - peak2.getValue()); + vpTRACE("No left and right valey for peak %d-%d...", peak2.getLevel(), peak2.getValue()); } else if (ret == 0x10) { - vpTRACE("No right valey for peak %d-%d...", peak2.getLevel(), - peak2.getValue()); + vpTRACE("No right valey for peak %d-%d...", peak2.getLevel(), peak2.getValue()); vpTRACE("Left valey: %d-%d", valeyl.getLevel(), valeyl.getValue()); } else if (ret == 0x01) { - vpTRACE("No left valey for peak %d-%d...", peak2.getLevel(), - peak2.getValue()); + vpTRACE("No left valey for peak %d-%d...", peak2.getLevel(), peak2.getValue()); vpTRACE("Right valey: %d-%d", valeyr.getLevel(), valeyr.getValue()); } else if (ret == 0x11) { vpTRACE("Left valey: %d-%d", valeyl.getLevel(), valeyl.getValue()); @@ -370,9 +355,8 @@ int main(int argc, const char **argv) if (h.getPeaks(distance, peakl, peakr, valey) == false) { std::cout << "Not a bimodal histogram..." << std::endl; } else { - vpTRACE("Bimodal histogram: valey %d-%d for peakl: %d-%d peakr: %d-%d", - valey.getLevel(), valey.getValue(), peakl.getLevel(), - peakl.getValue(), peakr.getLevel(), peakr.getValue()); + vpTRACE("Bimodal histogram: valey %d-%d for peakl: %d-%d peakr: %d-%d", valey.getLevel(), valey.getValue(), + peakl.getLevel(), peakl.getValue(), peakr.getLevel(), peakr.getValue()); } return 0; } catch (vpException &e) { diff --git a/example/tools/keyboard.cpp b/example/tools/keyboard.cpp index 74a97c6f97..6c43657153 100644 --- a/example/tools/keyboard.cpp +++ b/example/tools/keyboard.cpp @@ -45,8 +45,7 @@ #include #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) #include #include #include diff --git a/example/tools/parallelPort.cpp b/example/tools/parallelPort.cpp index 22bb4c50e6..830db42bae 100644 --- a/example/tools/parallelPort.cpp +++ b/example/tools/parallelPort.cpp @@ -115,8 +115,7 @@ bool getOptions(int argc, const char **argv, unsigned char &data) if ((value < 0) || (value > 255)) { usage(argv[0], optarg, data); std::cerr << "ERROR: " << std::endl; - std::cerr << " Bad value \"-d " << optarg << "\"" << std::endl - << std::endl; + std::cerr << " Bad value \"-d " << optarg << "\"" << std::endl << std::endl; return false; } else { data = (unsigned char)value; @@ -184,8 +183,7 @@ int main(int argc, const char **argv) #else int main() { - vpTRACE( - "Sorry, for the moment, vpParallelPort class works only on unix..."); + vpTRACE("Sorry, for the moment, vpParallelPort class works only on unix..."); return 0; } #endif diff --git a/example/tools/plot3d.cpp b/example/tools/plot3d.cpp index 703620c4d9..ed89004065 100644 --- a/example/tools/plot3d.cpp +++ b/example/tools/plot3d.cpp @@ -78,8 +78,7 @@ int main() unsigned long iter = 0; - std::cout << "Hit CTRL-C to or right mouse button to exit..." - << std::endl; + std::cout << "Hit CTRL-C to or right mouse button to exit..." << std::endl; bool end = false; while (!end) { if (iter < 300) { diff --git a/example/tracking/mbtEdgeKltMultiTracking.cpp b/example/tracking/mbtEdgeKltMultiTracking.cpp index e777888e0c..ee8fe68d57 100644 --- a/example/tracking/mbtEdgeKltMultiTracking.cpp +++ b/example/tracking/mbtEdgeKltMultiTracking.cpp @@ -47,9 +47,8 @@ #include #include -#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_MODULE_KLT) && \ - 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 #include @@ -148,12 +147,9 @@ OPTIONS: \n\ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); } -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &configFile, std::string &modelFile, - std::string &initFile, long &lastFrame, bool &displayFeatures, - bool &click_allowed, bool &display, bool &cao3DModel, - bool &trackCylinder, bool &useOgre, - bool &showOgreConfigDialog, bool &useScanline, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile, + std::string &initFile, long &lastFrame, bool &displayFeatures, bool &click_allowed, bool &display, + bool &cao3DModel, bool &trackCylinder, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, bool &computeCovariance, bool &projectionError) { const char *optarg_; @@ -263,11 +259,9 @@ int 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, opt_lastFrame, displayFeatures, - opt_click_allowed, opt_display, cao3DModel, trackCylinder, - useOgre, showOgreConfigDialog, useScanline, - computeCovariance, projectionError)) { + if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures, + opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, + useScanline, computeCovariance, projectionError)) { return (-1); } @@ -275,13 +269,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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; return (-1); } @@ -319,9 +310,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao); #endif } @@ -332,9 +321,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao); #endif } @@ -385,9 +372,7 @@ int main(int argc, const char **argv) display1.setDownScalingFactor(vpDisplay::SCALE_AUTO); display2.setDownScalingFactor(vpDisplay::SCALE_AUTO); display1.init(I1, 100, 100, "Test tracking (Left)"); - display2.init( - I2, (int)I1.getWidth() / vpDisplay::getDownScalingFactor(I1) + 110, - 100, "Test tracking (Right)"); + display2.init(I2, (int)I1.getWidth() / vpDisplay::getDownScalingFactor(I1) + 110, 100, "Test tracking (Right)"); #endif vpDisplay::display(I1); vpDisplay::display(I2); @@ -466,8 +451,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed) { while (!vpDisplay::getClick(I1, false)) { vpDisplay::display(I1); - vpDisplay::displayText( - I1, 15, 10, "click after positioning the object", vpColor::red); + vpDisplay::displayText(I1, 15, 10, "click after positioning the object", vpColor::red); vpDisplay::flush(I1); } } @@ -487,10 +471,8 @@ int main(int argc, const char **argv) // display the 3D model at the given pose tracker.display(I1, I2, c1Mo, c2Mo, cam1, cam2, vpColor::red); } else { - vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, - 2.063603907, 1.110231561, -0.4392789872); - vpHomogeneousMatrix c2Moi(0.02044769891, 0.1101505452, 0.5078963719, - 2.063603907, 1.110231561, -0.4392789872); + vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); + vpHomogeneousMatrix c2Moi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); tracker.initFromPose(I1, I2, c1Moi, c2Moi); } @@ -556,8 +538,7 @@ int main(int argc, const char **argv) // Specify the clipping to tracker.setNearClippingDistance(0.01); tracker.setFarClippingDistance(0.90); - tracker.setClipping(tracker.getClipping() | - vpMbtPolygon::FOV_CLIPPING); + tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING); // tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING | // vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | // vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING @@ -573,10 +554,8 @@ int main(int argc, const char **argv) // Test to set an initial pose if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) { - c1Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, - 0.8611798108, -0.3491961946); - c2Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, - 0.8611798108, -0.3491961946); + c1Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946); + c2Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946); std::cout << "Test set pose" << std::endl; tracker.setPose(I1, I2, c1Mo, c2Mo); } @@ -604,23 +583,18 @@ int main(int argc, const char **argv) } if (computeCovariance) { - std::cout << "Covariance matrix: \n" - << tracker.getCovarianceMatrix() << std::endl - << std::endl; + std::cout << "Covariance matrix: \n" << tracker.getCovarianceMatrix() << std::endl << std::endl; } if (projectionError) { - std::cout << "Projection error: " << tracker.getProjectionError() - << std::endl - << std::endl; + std::cout << "Projection error: " << tracker.getProjectionError() << std::endl << std::endl; } vpDisplay::flush(I1); vpDisplay::flush(I2); } - std::cout << "Reached last frame: " << reader.getFrameIndex() - << std::endl; + std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl; if (opt_click_allowed && !quit) { vpDisplay::getClick(I1); @@ -633,8 +607,7 @@ int main(int argc, const char **argv) vpXmlParser::cleanup(); #endif -#if defined(VISP_HAVE_COIN3D) && \ - (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3) +#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || 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) diff --git a/example/tracking/mbtEdgeKltTracking.cpp b/example/tracking/mbtEdgeKltTracking.cpp index 42a6be7ac0..15fb96da5c 100644 --- a/example/tracking/mbtEdgeKltTracking.cpp +++ b/example/tracking/mbtEdgeKltTracking.cpp @@ -46,9 +46,8 @@ #include #include -#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_MODULE_KLT) && \ - 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 #include @@ -147,12 +146,9 @@ OPTIONS: \n\ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); } -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &configFile, std::string &modelFile, - std::string &initFile, long &lastFrame, bool &displayFeatures, - bool &click_allowed, bool &display, bool &cao3DModel, - bool &trackCylinder, bool &useOgre, - bool &showOgreConfigDialog, bool &useScanline, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile, + std::string &initFile, long &lastFrame, bool &displayFeatures, bool &click_allowed, bool &display, + bool &cao3DModel, bool &trackCylinder, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, bool &computeCovariance, bool &projectionError) { const char *optarg_; @@ -262,11 +258,9 @@ int 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, opt_lastFrame, displayFeatures, - opt_click_allowed, opt_display, cao3DModel, trackCylinder, - useOgre, showOgreConfigDialog, useScanline, - computeCovariance, projectionError)) { + if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures, + opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, + useScanline, computeCovariance, projectionError)) { return (-1); } @@ -274,13 +268,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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; return (-1); } @@ -318,9 +309,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao); #endif } @@ -331,9 +320,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao); #endif } @@ -455,8 +442,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed) { while (!vpDisplay::getClick(I, false)) { vpDisplay::display(I); - vpDisplay::displayText( - I, 15, 10, "click after positioning the object", vpColor::red); + vpDisplay::displayText(I, 15, 10, "click after positioning the object", vpColor::red); vpDisplay::flush(I); vpTime::wait(100); } @@ -477,8 +463,7 @@ int main(int argc, const char **argv) // display the 3D model at the given pose tracker.display(I, cMo, cam, vpColor::red); } else { - vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, - 2.063603907, 1.110231561, -0.4392789872); + vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); tracker.initFromPose(I, cMoi); } @@ -536,8 +521,7 @@ int main(int argc, const char **argv) // Specify the clipping to tracker.setNearClippingDistance(0.01); tracker.setFarClippingDistance(0.90); - tracker.setClipping(tracker.getClipping() | - vpMbtPolygon::FOV_CLIPPING); + tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING); // tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING | // vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | // vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING @@ -553,8 +537,7 @@ int main(int argc, const char **argv) // Test to set an initial pose if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) { - cMo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, - 0.8611798108, -0.3491961946); + 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) { @@ -591,22 +574,17 @@ int main(int argc, const char **argv) } if (computeCovariance) { - std::cout << "Covariance matrix: \n" - << tracker.getCovarianceMatrix() << std::endl - << std::endl; + std::cout << "Covariance matrix: \n" << tracker.getCovarianceMatrix() << std::endl << std::endl; } if (projectionError) { - std::cout << "Projection error: " << tracker.getProjectionError() - << std::endl - << std::endl; + std::cout << "Projection error: " << tracker.getProjectionError() << std::endl << std::endl; } vpDisplay::flush(I); } - std::cout << "Reached last frame: " << reader.getFrameIndex() - << std::endl; + std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl; if (opt_click_allowed && !quit) { vpDisplay::getClick(I); diff --git a/example/tracking/mbtEdgeMultiTracking.cpp b/example/tracking/mbtEdgeMultiTracking.cpp index ad762220e8..a79556ab34 100644 --- a/example/tracking/mbtEdgeMultiTracking.cpp +++ b/example/tracking/mbtEdgeMultiTracking.cpp @@ -148,12 +148,9 @@ OPTIONS: \n\ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); } -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &configFile, std::string &modelFile, - std::string &initFile, long &lastFrame, bool &displayFeatures, - bool &click_allowed, bool &display, bool &cao3DModel, - bool &trackCylinder, bool &useOgre, - bool &showOgreConfigDialog, bool &useScanline, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile, + std::string &initFile, long &lastFrame, bool &displayFeatures, bool &click_allowed, bool &display, + bool &cao3DModel, bool &trackCylinder, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, bool &computeCovariance, bool &projectionError) { const char *optarg_; @@ -263,11 +260,9 @@ int 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, opt_lastFrame, displayFeatures, - opt_click_allowed, opt_display, cao3DModel, trackCylinder, - useOgre, showOgreConfigDialog, useScanline, - computeCovariance, projectionError)) { + if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures, + opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, + useScanline, computeCovariance, projectionError)) { return (-1); } @@ -275,13 +270,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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; return (-1); } @@ -319,9 +311,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao); #endif } @@ -332,9 +322,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao); #endif } @@ -385,10 +373,7 @@ int main(int argc, const char **argv) display1.setDownScalingFactor(vpDisplay::SCALE_AUTO); display2.setDownScalingFactor(vpDisplay::SCALE_AUTO); display1.init(I1, 100, 100, "Test tracking (Left)"); - display2.init( - I2, - (int)(I1.getWidth() / vpDisplay::getDownScalingFactor(I1) + 110), - 100, "Test tracking (Right)"); + display2.init(I2, (int)(I1.getWidth() / vpDisplay::getDownScalingFactor(I1) + 110), 100, "Test tracking (Right)"); #endif vpDisplay::display(I1); vpDisplay::display(I2); @@ -461,8 +446,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed) { while (!vpDisplay::getClick(I1, false)) { vpDisplay::display(I1); - vpDisplay::displayText( - I1, 15, 10, "click after positioning the object", vpColor::red); + vpDisplay::displayText(I1, 15, 10, "click after positioning the object", vpColor::red); vpDisplay::flush(I1); vpTime::wait(100); } @@ -483,10 +467,8 @@ int main(int argc, const char **argv) // display the 3D model at the given pose tracker.display(I1, I2, c1Mo, c2Mo, cam1, cam2, vpColor::red); } else { - vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, - 2.063603907, 1.110231561, -0.4392789872); - vpHomogeneousMatrix c2Moi(0.02044769891, 0.1101505452, 0.5078963719, - 2.063603907, 1.110231561, -0.4392789872); + vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); + vpHomogeneousMatrix c2Moi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); tracker.initFromPose(I1, I2, c1Moi, c2Moi); } @@ -540,8 +522,7 @@ int main(int argc, const char **argv) // Specify the clipping to use tracker.setNearClippingDistance(0.01); tracker.setFarClippingDistance(0.90); - tracker.setClipping(tracker.getClipping() | - vpMbtPolygon::FOV_CLIPPING); + tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING); // tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING | // vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | // vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING @@ -557,10 +538,8 @@ int main(int argc, const char **argv) // Test to set an initial pose if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) { - c1Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, - 0.8611798108, -0.3491961946); - c2Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, - 0.8611798108, -0.3491961946); + c1Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946); + c2Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946); std::cout << "Test set pose" << std::endl; tracker.setPose(I1, I2, c1Mo, c2Mo); } @@ -588,23 +567,18 @@ int main(int argc, const char **argv) } if (computeCovariance) { - std::cout << "Covariance matrix: \n" - << tracker.getCovarianceMatrix() << std::endl - << std::endl; + std::cout << "Covariance matrix: \n" << tracker.getCovarianceMatrix() << std::endl << std::endl; } if (projectionError) { - std::cout << "Projection error: " << tracker.getProjectionError() - << std::endl - << std::endl; + std::cout << "Projection error: " << tracker.getProjectionError() << std::endl << std::endl; } vpDisplay::flush(I1); vpDisplay::flush(I2); } - std::cout << "Reached last frame: " << reader.getFrameIndex() - << std::endl; + std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl; if (opt_click_allowed && !quit) { vpDisplay::getClick(I1); @@ -617,8 +591,7 @@ int main(int argc, const char **argv) vpXmlParser::cleanup(); #endif -#if defined(VISP_HAVE_COIN3D) && \ - (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3) +#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || 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) @@ -636,8 +609,7 @@ int main(int argc, const char **argv) int main() { - std::cout << "visp_mbt module 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/mbtEdgeTracking.cpp b/example/tracking/mbtEdgeTracking.cpp index 3cd877b5fd..299a2e45e1 100644 --- a/example/tracking/mbtEdgeTracking.cpp +++ b/example/tracking/mbtEdgeTracking.cpp @@ -147,12 +147,9 @@ OPTIONS: \n\ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); } -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &configFile, std::string &modelFile, - std::string &initFile, long &lastFrame, bool &displayFeatures, - bool &click_allowed, bool &display, bool &cao3DModel, - bool &trackCylinder, bool &useOgre, - bool &showOgreConfigDialog, bool &useScanline, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile, + std::string &initFile, long &lastFrame, bool &displayFeatures, bool &click_allowed, bool &display, + bool &cao3DModel, bool &trackCylinder, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, bool &computeCovariance, bool &projectionError) { const char *optarg_; @@ -262,11 +259,9 @@ int 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, opt_lastFrame, displayFeatures, - opt_click_allowed, opt_display, cao3DModel, trackCylinder, - useOgre, showOgreConfigDialog, useScanline, - computeCovariance, projectionError)) { + if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures, + opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, + useScanline, computeCovariance, projectionError)) { return (-1); } @@ -274,13 +269,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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; return (-1); } @@ -318,9 +310,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao); #endif } @@ -331,9 +321,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao); #endif } @@ -443,8 +431,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed) { while (!vpDisplay::getClick(I, false)) { vpDisplay::display(I); - vpDisplay::displayText( - I, 15, 10, "click after positioning the object", vpColor::red); + vpDisplay::displayText(I, 15, 10, "click after positioning the object", vpColor::red); vpDisplay::flush(I); } } @@ -464,8 +451,7 @@ int main(int argc, const char **argv) // display the 3D model at the given pose tracker.display(I, cMo, cam, vpColor::red); } else { - vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, - 2.063603907, 1.110231561, -0.4392789872); + vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); tracker.initFromPose(I, cMoi); } @@ -510,8 +496,7 @@ int main(int argc, const char **argv) // Specify the clipping to use tracker.setNearClippingDistance(0.01); tracker.setFarClippingDistance(0.90); - tracker.setClipping(tracker.getClipping() | - vpMbtPolygon::FOV_CLIPPING); + tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING); // tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING | // vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | // vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING @@ -527,8 +512,7 @@ int main(int argc, const char **argv) // Test to set an initial pose if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) { - cMo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, - 0.8611798108, -0.3491961946); + 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) { @@ -565,22 +549,17 @@ int main(int argc, const char **argv) } if (computeCovariance) { - std::cout << "Covariance matrix: \n" - << tracker.getCovarianceMatrix() << std::endl - << std::endl; + std::cout << "Covariance matrix: \n" << tracker.getCovarianceMatrix() << std::endl << std::endl; } if (projectionError) { - std::cout << "Projection error: " << tracker.getProjectionError() - << std::endl - << std::endl; + std::cout << "Projection error: " << tracker.getProjectionError() << std::endl << std::endl; } vpDisplay::flush(I); } - std::cout << "Reached last frame: " << reader.getFrameIndex() - << std::endl; + std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl; if (opt_click_allowed && !quit) { vpDisplay::getClick(I); @@ -611,8 +590,7 @@ int main(int argc, const char **argv) int main() { - std::cout << "visp_mbt module 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/mbtGenericTracking.cpp b/example/tracking/mbtGenericTracking.cpp index 33b2e79f52..cdc9681d17 100644 --- a/example/tracking/mbtGenericTracking.cpp +++ b/example/tracking/mbtGenericTracking.cpp @@ -152,14 +152,10 @@ OPTIONS: \n\ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); } -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &configFile, std::string &modelFile, - std::string &initFile, long &lastFrame, bool &displayFeatures, - bool &click_allowed, bool &display, bool &cao3DModel, - bool &trackCylinder, bool &useOgre, - bool &showOgreConfigDialog, bool &useScanline, - bool &computeCovariance, bool &projectionError, - int &trackerType) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile, + std::string &initFile, long &lastFrame, bool &displayFeatures, bool &click_allowed, bool &display, + bool &cao3DModel, bool &trackCylinder, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, + bool &computeCovariance, bool &projectionError, int &trackerType) { const char *optarg_; int c; @@ -270,11 +266,9 @@ int 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, opt_lastFrame, displayFeatures, - opt_click_allowed, opt_display, cao3DModel, trackCylinder, - useOgre, showOgreConfigDialog, useScanline, - computeCovariance, projectionError, trackerType)) { + if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures, + opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, + useScanline, computeCovariance, projectionError, trackerType)) { return EXIT_FAILURE; } @@ -282,13 +276,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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; return EXIT_FAILURE; } @@ -329,9 +320,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao); #endif } @@ -342,9 +331,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao); #endif } @@ -395,10 +382,7 @@ int main(int argc, const char **argv) display1.setDownScalingFactor(vpDisplay::SCALE_AUTO); display2.setDownScalingFactor(vpDisplay::SCALE_AUTO); display1.init(I1, 100, 100, "Test tracking (Left)"); - display2.init( - I2, - (int)(I1.getWidth() / vpDisplay::getDownScalingFactor(I1)) + 110, - 100, "Test tracking (Right)"); + display2.init(I2, (int)(I1.getWidth() / vpDisplay::getDownScalingFactor(I1)) + 110, 100, "Test tracking (Right)"); #endif vpDisplay::display(I1); vpDisplay::display(I2); @@ -413,8 +397,7 @@ int main(int argc, const char **argv) // Initialise the tracker: camera parameters, moving edge and KLT settings #if defined(VISP_HAVE_XML2) && USE_XML // From the xml file - dynamic_cast(tracker)->loadConfigFile(configFile, - configFile); + dynamic_cast(tracker)->loadConfigFile(configFile, configFile); #else // By setting the parameters: cam1.initPersProjWithoutDistortion(547, 542, 338, 234); @@ -429,8 +412,7 @@ int main(int argc, const char **argv) me.setMu2(0.5); me.setSampleStep(4); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) vpKltOpencv klt; klt.setMaxFeatures(10000); klt.setWindowSize(5); @@ -444,8 +426,7 @@ int main(int argc, const char **argv) dynamic_cast(tracker)->setKltMaskBorder(5); #endif - dynamic_cast(tracker)->setCameraParameters(cam1, - cam2); + dynamic_cast(tracker)->setCameraParameters(cam1, cam2); dynamic_cast(tracker)->setMovingEdge(me); tracker->setAngleAppear(vpMath::rad(65)); @@ -478,15 +459,13 @@ int main(int argc, const char **argv) tracker->setProjectionErrorComputation(projectionError); // Retrieve the camera parameters from the tracker - dynamic_cast(tracker)->getCameraParameters(cam1, - cam2); + dynamic_cast(tracker)->getCameraParameters(cam1, cam2); // Loop to position the cube if (opt_display && opt_click_allowed) { while (!vpDisplay::getClick(I1, false)) { vpDisplay::display(I1); - vpDisplay::displayText( - I1, 15, 10, "click after positioning the object", vpColor::red); + vpDisplay::displayText(I1, 15, 10, "click after positioning the object", vpColor::red); vpDisplay::flush(I1); } } @@ -501,19 +480,14 @@ int main(int argc, const char **argv) // - a ./cube/cube.ppm file to display where the user have to click // (optionnal, set by the third parameter) if (opt_display && opt_click_allowed) { - dynamic_cast(tracker)->initClick(I1, I2, initFile, - initFile, true); + dynamic_cast(tracker)->initClick(I1, I2, initFile, initFile, true); dynamic_cast(tracker)->getPose(c1Mo, c2Mo); // display the 3D model at the given pose - dynamic_cast(tracker)->display( - I1, I2, c1Mo, c2Mo, cam1, cam2, vpColor::red); + dynamic_cast(tracker)->display(I1, I2, c1Mo, c2Mo, cam1, cam2, vpColor::red); } else { - vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, - 2.063603907, 1.110231561, -0.4392789872); - vpHomogeneousMatrix c2Moi(0.02044769891, 0.1101505452, 0.5078963719, - 2.063603907, 1.110231561, -0.4392789872); - dynamic_cast(tracker)->initFromPose(I1, I2, c1Moi, - c2Moi); + vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); + vpHomogeneousMatrix c2Moi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); + dynamic_cast(tracker)->initFromPose(I1, I2, c1Moi, c2Moi); } // track the model @@ -536,8 +510,7 @@ int main(int argc, const char **argv) vpDisplay::display(I2); std::stringstream ss; - ss << "Num frame: " << reader.getFrameIndex() << "/" - << reader.getLastFrameIndex(); + ss << "Num frame: " << reader.getFrameIndex() << "/" << reader.getLastFrameIndex(); vpDisplay::displayText(I1, 40, 20, ss.str(), vpColor::red); } @@ -551,8 +524,7 @@ int main(int argc, const char **argv) tracker->resetTracker(); #if defined(VISP_HAVE_XML2) && USE_XML - dynamic_cast(tracker)->loadConfigFile( - configFile, configFile); + dynamic_cast(tracker)->loadConfigFile(configFile, configFile); #else // By setting the parameters: cam1.initPersProjWithoutDistortion(547, 542, 338, 234); @@ -567,8 +539,7 @@ int main(int argc, const char **argv) me.setMu2(0.5); me.setSampleStep(4); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) vpKltOpencv klt; klt.setMaxFeatures(10000); klt.setWindowSize(5); @@ -582,8 +553,7 @@ int main(int argc, const char **argv) dynamic_cast(tracker)->setKltMaskBorder(5); #endif - dynamic_cast(tracker)->setCameraParameters( - cam1, cam2); + dynamic_cast(tracker)->setCameraParameters(cam1, cam2); dynamic_cast(tracker)->setMovingEdge(me); tracker->setAngleAppear(vpMath::rad(65)); tracker->setAngleDisappear(vpMath::rad(75)); @@ -591,32 +561,26 @@ int main(int argc, const char **argv) // Specify the clipping to tracker->setNearClippingDistance(0.01); tracker->setFarClippingDistance(0.90); - tracker->setClipping(tracker->getClipping() | - vpMbtPolygon::FOV_CLIPPING); + tracker->setClipping(tracker->getClipping() | vpMbtPolygon::FOV_CLIPPING); // tracker->setClipping(tracker->getClipping() | vpMbtPolygon::LEFT_CLIPPING // | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | // vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING #endif tracker->loadModel(modelFile); - dynamic_cast(tracker)->setCameraParameters( - cam1, cam2); + dynamic_cast(tracker)->setCameraParameters(cam1, cam2); tracker->setOgreVisibilityTest(useOgre); tracker->setScanLineVisibilityTest(useScanline); tracker->setCovarianceComputation(computeCovariance); tracker->setProjectionErrorComputation(projectionError); - dynamic_cast(tracker)->initFromPose(I1, I2, - c1Mo, c2Mo); + dynamic_cast(tracker)->initFromPose(I1, I2, c1Mo, c2Mo); } // Test to set an initial pose if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) { - c1Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, - 0.8611798108, -0.3491961946); - c2Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, - 0.8611798108, -0.3491961946); + c1Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946); + c2Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946); std::cout << "Test set pose" << std::endl; - dynamic_cast(tracker)->setPose(I1, I2, c1Mo, - c2Mo); + dynamic_cast(tracker)->setPose(I1, I2, c1Mo, c2Mo); } // track the object: stop tracking from frame 40 to 50 @@ -626,8 +590,7 @@ int main(int argc, const char **argv) dynamic_cast(tracker)->getPose(c1Mo, c2Mo); if (opt_display) { // display the 3D model - dynamic_cast(tracker)->display( - I1, I2, c1Mo, c2Mo, cam1, cam2, vpColor::darkRed); + dynamic_cast(tracker)->display(I1, I2, c1Mo, c2Mo, cam1, cam2, vpColor::darkRed); // display the frame vpDisplay::displayFrame(I1, c1Mo, cam1, 0.05); vpDisplay::displayFrame(I2, c2Mo, cam2, 0.05); @@ -654,26 +617,19 @@ int main(int argc, const char **argv) } if (computeCovariance) { - std::cout << "Covariance matrix: \n" - << tracker->getCovarianceMatrix() << std::endl - << std::endl; + std::cout << "Covariance matrix: \n" << tracker->getCovarianceMatrix() << std::endl << std::endl; } if (projectionError) { - std::cout << "Projection error: " << tracker->getProjectionError() - << std::endl - << std::endl; + std::cout << "Projection error: " << tracker->getProjectionError() << std::endl << std::endl; } vpDisplay::flush(I1); vpDisplay::flush(I2); } - std::cout << "Reached last frame: " << reader.getFrameIndex() - << std::endl; - std::cout << "\nFinal poses, c1Mo:\n" - << c1Mo << "\nc2Mo:\n" - << c2Mo << std::endl; + std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl; + std::cout << "\nFinal poses, c1Mo:\n" << c1Mo << "\nc2Mo:\n" << c2Mo << std::endl; if (opt_click_allowed && !quit) { vpDisplay::getClick(I1); @@ -689,8 +645,7 @@ int main(int argc, const char **argv) vpXmlParser::cleanup(); #endif -#if defined(VISP_HAVE_COIN3D) && \ - (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3) +#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3) // Cleanup memory allocated by Coin library used to load a vrml model in // vpMbGenericTracker::loadModel() We clean only if Coin was used. if (!cao3DModel) diff --git a/example/tracking/mbtGenericTracking2.cpp b/example/tracking/mbtGenericTracking2.cpp index 3a243b9c11..4200b58da7 100644 --- a/example/tracking/mbtGenericTracking2.cpp +++ b/example/tracking/mbtGenericTracking2.cpp @@ -152,14 +152,10 @@ OPTIONS: \n\ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); } -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &configFile, std::string &modelFile, - std::string &initFile, long &lastFrame, bool &displayFeatures, - bool &click_allowed, bool &display, bool &cao3DModel, - bool &trackCylinder, bool &useOgre, - bool &showOgreConfigDialog, bool &useScanline, - bool &computeCovariance, bool &projectionError, - int &trackerType) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile, + std::string &initFile, long &lastFrame, bool &displayFeatures, bool &click_allowed, bool &display, + bool &cao3DModel, bool &trackCylinder, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, + bool &computeCovariance, bool &projectionError, int &trackerType) { const char *optarg_; int c; @@ -270,11 +266,9 @@ int 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, opt_lastFrame, displayFeatures, - opt_click_allowed, opt_display, cao3DModel, trackCylinder, - useOgre, showOgreConfigDialog, useScanline, - computeCovariance, projectionError, trackerType)) { + if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures, + opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, + useScanline, computeCovariance, projectionError, trackerType)) { return EXIT_FAILURE; } @@ -282,13 +276,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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; return EXIT_FAILURE; } @@ -329,9 +320,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao); #endif } @@ -342,9 +331,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao); #endif } @@ -405,14 +392,8 @@ int main(int argc, const char **argv) display3.setDownScalingFactor(vpDisplay::SCALE_AUTO); display1.init(I1, 100, 100, "Test tracking (Cam1)"); - display2.init( - I2, - (int)(I1.getWidth() / vpDisplay::getDownScalingFactor(I1)) + 110, - 100, "Test tracking (Cam2)"); - display3.init( - I3, 100, - (int)(I1.getHeight() / vpDisplay::getDownScalingFactor(I1)) + 110, - "Test tracking (Cam3)"); + display2.init(I2, (int)(I1.getWidth() / vpDisplay::getDownScalingFactor(I1)) + 110, 100, "Test tracking (Cam2)"); + display3.init(I3, 100, (int)(I1.getHeight() / vpDisplay::getDownScalingFactor(I1)) + 110, "Test tracking (Cam3)"); #endif vpDisplay::display(I1); vpDisplay::display(I2); @@ -434,8 +415,7 @@ int main(int argc, const char **argv) mapOfConfigFiles["Camera1"] = configFile; mapOfConfigFiles["Camera2"] = configFile; mapOfConfigFiles["Camera3"] = configFile; - dynamic_cast(tracker)->loadConfigFile( - mapOfConfigFiles); + dynamic_cast(tracker)->loadConfigFile(mapOfConfigFiles); #else // By setting the parameters: vpCameraParameters cam; @@ -457,8 +437,7 @@ int main(int argc, const char **argv) mapOfMe["Camera2"] = me; mapOfMe["Camera3"] = me; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) vpKltOpencv klt; klt.setMaxFeatures(10000); klt.setWindowSize(5); @@ -476,8 +455,7 @@ int main(int argc, const char **argv) dynamic_cast(tracker)->setKltMaskBorder(5); #endif - dynamic_cast(tracker)->setCameraParameters( - mapOfCameraParams); + dynamic_cast(tracker)->setCameraParameters(mapOfCameraParams); dynamic_cast(tracker)->setMovingEdge(mapOfMe); tracker->setAngleAppear(vpMath::rad(65)); tracker->setAngleDisappear(vpMath::rad(75)); @@ -487,16 +465,13 @@ int main(int argc, const char **argv) tracker->setFarClippingDistance(0.90); std::map mapOfClippingFlags; - dynamic_cast(tracker)->getClipping( - mapOfClippingFlags); - for (std::map::iterator it = - mapOfClippingFlags.begin(); - it != mapOfClippingFlags.end(); ++it) { + dynamic_cast(tracker)->getClipping(mapOfClippingFlags); + for (std::map::iterator it = mapOfClippingFlags.begin(); it != mapOfClippingFlags.end(); + ++it) { it->second = (it->second | vpMbtPolygon::FOV_CLIPPING); } - dynamic_cast(tracker)->setClipping( - mapOfClippingFlags); + dynamic_cast(tracker)->setClipping(mapOfClippingFlags); // tracker->setClipping(tracker->getClipping() | vpMbtPolygon::LEFT_CLIPPING // | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | // vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING @@ -520,15 +495,13 @@ int main(int argc, const char **argv) tracker->setProjectionErrorComputation(projectionError); // Retrieve the camera parameters from the tracker - dynamic_cast(tracker)->getCameraParameters( - mapOfCameraParams); + dynamic_cast(tracker)->getCameraParameters(mapOfCameraParams); // Loop to position the cube if (opt_display && opt_click_allowed) { while (!vpDisplay::getClick(I1, false)) { vpDisplay::display(I1); - vpDisplay::displayText( - I1, 15, 10, "click after positioning the object", vpColor::red); + vpDisplay::displayText(I1, 15, 10, "click after positioning the object", vpColor::red); vpDisplay::flush(I1); } } @@ -546,21 +519,18 @@ int main(int argc, const char **argv) std::map mapOfInitFiles; mapOfInitFiles["Camera1"] = initFile; - dynamic_cast(tracker)->initClick( - mapOfImages, mapOfInitFiles, true); + dynamic_cast(tracker)->initClick(mapOfImages, mapOfInitFiles, true); dynamic_cast(tracker)->getPose(mapOfCameraPoses); // display the 3D model at the given pose - dynamic_cast(tracker)->display( - mapOfImages, mapOfCameraPoses, mapOfCameraParams, vpColor::red); + dynamic_cast(tracker)->display(mapOfImages, mapOfCameraPoses, mapOfCameraParams, + vpColor::red); } else { - vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, - 2.063603907, 1.110231561, -0.4392789872); + vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); std::map mapOfInitPoses; mapOfInitPoses["Camera1"] = c1Moi; - dynamic_cast(tracker)->initFromPose( - mapOfImages, mapOfInitPoses); + dynamic_cast(tracker)->initFromPose(mapOfImages, mapOfInitPoses); } // track the model @@ -590,8 +560,7 @@ int main(int argc, const char **argv) vpDisplay::display(I3); std::stringstream ss; - ss << "Num frame: " << reader.getFrameIndex() << "/" - << reader.getLastFrameIndex(); + ss << "Num frame: " << reader.getFrameIndex() << "/" << reader.getLastFrameIndex(); vpDisplay::displayText(I1, 40, 20, ss.str(), vpColor::red); } @@ -606,8 +575,7 @@ int main(int argc, const char **argv) tracker->resetTracker(); #if defined(VISP_HAVE_XML2) && USE_XML - dynamic_cast(tracker)->loadConfigFile( - mapOfConfigFiles); + dynamic_cast(tracker)->loadConfigFile(mapOfConfigFiles); #else // By setting the parameters: vpCameraParameters cam; @@ -629,8 +597,7 @@ int main(int argc, const char **argv) mapOfMe["Camera2"] = me; mapOfMe["Camera3"] = me; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) vpKltOpencv klt; klt.setMaxFeatures(10000); klt.setWindowSize(5); @@ -648,8 +615,7 @@ int main(int argc, const char **argv) dynamic_cast(tracker)->setKltMaskBorder(5); #endif - dynamic_cast(tracker)->setCameraParameters( - mapOfCameraParams); + dynamic_cast(tracker)->setCameraParameters(mapOfCameraParams); dynamic_cast(tracker)->setMovingEdge(mapOfMe); tracker->setAngleAppear(vpMath::rad(65)); tracker->setAngleDisappear(vpMath::rad(75)); @@ -659,73 +625,59 @@ int main(int argc, const char **argv) tracker->setFarClippingDistance(0.90); std::map mapOfClippingFlags; - dynamic_cast(tracker)->getClipping( - mapOfClippingFlags); - for (std::map::iterator it = - mapOfClippingFlags.begin(); + dynamic_cast(tracker)->getClipping(mapOfClippingFlags); + for (std::map::iterator it = mapOfClippingFlags.begin(); it != mapOfClippingFlags.end(); ++it) { it->second = (it->second | vpMbtPolygon::FOV_CLIPPING); } - dynamic_cast(tracker)->setClipping( - mapOfClippingFlags); + dynamic_cast(tracker)->setClipping(mapOfClippingFlags); // tracker->setClipping(tracker->getClipping() | vpMbtPolygon::LEFT_CLIPPING // | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | // vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING #endif tracker->loadModel(modelFile); - dynamic_cast(tracker)->setCameraParameters( - mapOfCameraParams); + dynamic_cast(tracker)->setCameraParameters(mapOfCameraParams); tracker->setOgreVisibilityTest(useOgre); tracker->setScanLineVisibilityTest(useScanline); tracker->setCovarianceComputation(computeCovariance); tracker->setProjectionErrorComputation(projectionError); - dynamic_cast(tracker)->initFromPose( - mapOfImages, mapOfCameraPoses); + dynamic_cast(tracker)->initFromPose(mapOfImages, mapOfCameraPoses); } // Test to set an initial pose if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) { vpHomogeneousMatrix c1Moi; - c1Moi.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, - 0.8611798108, -0.3491961946); + c1Moi.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946); std::map mapOfSetPoses; mapOfSetPoses["Camera1"] = c1Moi; std::cout << "Test set pose" << std::endl; - dynamic_cast(tracker)->setPose(mapOfImages, - mapOfSetPoses); + dynamic_cast(tracker)->setPose(mapOfImages, mapOfSetPoses); } // track the object: stop tracking from frame 40 to 50 if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) { dynamic_cast(tracker)->track(mapOfImages); - dynamic_cast(tracker)->getPose( - mapOfCameraPoses); + dynamic_cast(tracker)->getPose(mapOfCameraPoses); if (opt_display) { // display the 3D model if (reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) { - std::map *> - mapOfSubImages; + std::map *> mapOfSubImages; mapOfSubImages["Camera1"] = &I1; mapOfSubImages["Camera2"] = &I2; - dynamic_cast(tracker)->display( - mapOfSubImages, mapOfCameraPoses, mapOfCameraParams, - vpColor::red, 3); + dynamic_cast(tracker)->display(mapOfSubImages, mapOfCameraPoses, mapOfCameraParams, + vpColor::red, 3); } else { - dynamic_cast(tracker)->display( - mapOfImages, mapOfCameraPoses, mapOfCameraParams, - vpColor::red, 3); + dynamic_cast(tracker)->display(mapOfImages, mapOfCameraPoses, mapOfCameraParams, + vpColor::red, 3); } // display the frame - vpDisplay::displayFrame(I1, mapOfCameraPoses["Camera1"], - mapOfCameraParams["Camera1"], 0.05); - vpDisplay::displayFrame(I2, mapOfCameraPoses["Camera2"], - mapOfCameraParams["Camera2"], 0.05); - vpDisplay::displayFrame(I3, mapOfCameraPoses["Camera3"], - mapOfCameraParams["Camera3"], 0.05); + vpDisplay::displayFrame(I1, mapOfCameraPoses["Camera1"], mapOfCameraParams["Camera1"], 0.05); + vpDisplay::displayFrame(I2, mapOfCameraPoses["Camera2"], mapOfCameraParams["Camera2"], 0.05); + vpDisplay::displayFrame(I3, mapOfCameraPoses["Camera3"], mapOfCameraParams["Camera3"], 0.05); } } @@ -749,15 +701,11 @@ int main(int argc, const char **argv) } if (computeCovariance) { - std::cout << "Covariance matrix: \n" - << tracker->getCovarianceMatrix() << std::endl - << std::endl; + std::cout << "Covariance matrix: \n" << tracker->getCovarianceMatrix() << std::endl << std::endl; } if (projectionError) { - std::cout << "Projection error: " << tracker->getProjectionError() - << std::endl - << std::endl; + std::cout << "Projection error: " << tracker->getProjectionError() << std::endl << std::endl; } vpDisplay::flush(I1); @@ -765,8 +713,7 @@ int main(int argc, const char **argv) vpDisplay::flush(I3); } - std::cout << "Reached last frame: " << reader.getFrameIndex() - << std::endl; + std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl; std::cout << "\nFinal poses, c1Mo:\n" << mapOfCameraPoses["Camera1"] << "\nc2Mo:\n" << mapOfCameraPoses["Camera2"] << "\nc3Mo:\n" @@ -786,8 +733,7 @@ int main(int argc, const char **argv) vpXmlParser::cleanup(); #endif -#if defined(VISP_HAVE_COIN3D) && \ - (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3) +#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3) // Cleanup memory allocated by Coin library used to load a vrml model in // vpMbGenericTracker::loadModel() We clean only if Coin was used. if (!cao3DModel) diff --git a/example/tracking/mbtGenericTrackingDepth.cpp b/example/tracking/mbtGenericTrackingDepth.cpp index 32810bf85c..dc026391ff 100644 --- a/example/tracking/mbtGenericTrackingDepth.cpp +++ b/example/tracking/mbtGenericTrackingDepth.cpp @@ -149,14 +149,11 @@ void usage(const char *name, const char *badparam) fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); } -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &configFile, std::string &configFile_depth, - std::string &modelFile, std::string &modelFile_depth, - std::string &initFile, bool &displayFeatures, - bool &click_allowed, bool &display, bool &useOgre, - bool &showOgreConfigDialog, bool &useScanline, - bool &computeCovariance, bool &projectionError, - int &trackerType, int &tracker_type_depth, int &lastFrame) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &configFile_depth, + std::string &modelFile, std::string &modelFile_depth, std::string &initFile, bool &displayFeatures, + bool &click_allowed, bool &display, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, + bool &computeCovariance, bool &projectionError, int &trackerType, int &tracker_type_depth, + int &lastFrame) { const char *optarg_; int c; @@ -238,31 +235,26 @@ bool getOptions(int argc, const char **argv, std::string &ipath, } struct rs_intrinsics { - float ppx; /**< Horizontal coordinate of the principal point of the image, - as a pixel offset from the left edge */ - float ppy; /**< Vertical coordinate of the principal point of the image, as - a pixel offset from the top edge */ - float - fx; /**< Focal length of the image plane, as a multiple of pixel width - */ - float fy; /**< Focal length of the image plane, as a multiple of pixel - height */ + float ppx; /**< Horizontal coordinate of the principal point of the image, + as a pixel offset from the left edge */ + float ppy; /**< Vertical coordinate of the principal point of the image, as + a pixel offset from the top edge */ + float fx; /**< Focal length of the image plane, as a multiple of pixel width + */ + float fy; /**< Focal length of the image plane, as a multiple of pixel + height */ float coeffs[5]; /**< Distortion coefficients */ }; -void rs_deproject_pixel_to_point(float point[3], const rs_intrinsics &intrin, - const float pixel[2], float depth) +void rs_deproject_pixel_to_point(float point[3], const rs_intrinsics &intrin, const float pixel[2], float depth) { float x = (pixel[0] - intrin.ppx) / intrin.fx; float y = (pixel[1] - intrin.ppy) / intrin.fy; float r2 = x * x + y * y; - float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + - intrin.coeffs[4] * r2 * r2 * r2; - float ux = x * f + 2 * intrin.coeffs[2] * x * y + - intrin.coeffs[3] * (r2 + 2 * x * x); - float uy = y * f + 2 * intrin.coeffs[3] * x * y + - intrin.coeffs[2] * (r2 + 2 * y * y); + float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2; + float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x); + float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y); x = ux; y = uy; @@ -272,10 +264,8 @@ void rs_deproject_pixel_to_point(float point[3], const rs_intrinsics &intrin, point[2] = depth; } -bool read_data(const unsigned int cpt, const std::string &input_directory, - vpImage &I, vpImage &I_depth_raw, - std::vector &pointcloud, - unsigned int &pointcloud_width, +bool read_data(const unsigned int cpt, const std::string &input_directory, vpImage &I, + vpImage &I_depth_raw, std::vector &pointcloud, unsigned int &pointcloud_width, unsigned int &pointcloud_height) { char buffer[256]; @@ -298,8 +288,7 @@ bool read_data(const unsigned int cpt, const std::string &input_directory, sprintf(buffer, ss.str().c_str(), cpt); std::string filename_depth = buffer; - std::ifstream file_depth(filename_depth.c_str(), - std::ios::in | std::ios::binary); + std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary); if (!file_depth.is_open()) { return false; } @@ -340,8 +329,7 @@ bool read_data(const unsigned int cpt, const std::string &input_directory, float scaled_depth = I_depth_raw[i][j] * depth_scale; float point[3]; float pixel[2] = {(float)j, (float)i}; - rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, - scaled_depth); + rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth); vpColVector data_3D(3); data_3D[0] = point[0]; @@ -369,8 +357,7 @@ void loadConfiguration(vpMbTracker *const tracker, { #if defined(VISP_HAVE_XML2) && USE_XML // From the xml file - dynamic_cast(tracker)->loadConfigFile( - configFile, configFile_depth); + dynamic_cast(tracker)->loadConfigFile(configFile, configFile_depth); #else // Edge vpMe me; @@ -384,8 +371,7 @@ void loadConfiguration(vpMbTracker *const tracker, dynamic_cast(tracker)->setMovingEdge(me); // Klt -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) vpKltOpencv klt; klt.setMaxFeatures(10000); klt.setWindowSize(5); @@ -400,29 +386,20 @@ void loadConfiguration(vpMbTracker *const tracker, #endif // Depth - dynamic_cast(tracker) - ->setDepthNormalFeatureEstimationMethod( - vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION); - dynamic_cast(tracker) - ->setDepthNormalPclPlaneEstimationMethod(2); - dynamic_cast(tracker) - ->setDepthNormalPclPlaneEstimationRansacMaxIter(200); - dynamic_cast(tracker) - ->setDepthNormalPclPlaneEstimationRansacThreshold(0.001); - dynamic_cast(tracker)->setDepthNormalSamplingStep(2, - 2); - - dynamic_cast(tracker)->setDepthDenseSamplingStep(4, - 4); + dynamic_cast(tracker)->setDepthNormalFeatureEstimationMethod( + vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION); + dynamic_cast(tracker)->setDepthNormalPclPlaneEstimationMethod(2); + dynamic_cast(tracker)->setDepthNormalPclPlaneEstimationRansacMaxIter(200); + dynamic_cast(tracker)->setDepthNormalPclPlaneEstimationRansacThreshold(0.001); + dynamic_cast(tracker)->setDepthNormalSamplingStep(2, 2); + + dynamic_cast(tracker)->setDepthDenseSamplingStep(4, 4); vpCameraParameters cam1, cam2; - cam1.initPersProjWithoutDistortion(615.1674804688, 615.1675415039, - 312.1889953613, 243.4373779297); - cam2.initPersProjWithoutDistortion(476.0536193848, 476.0534973145, - 311.4845581055, 246.2832336426); + cam1.initPersProjWithoutDistortion(615.1674804688, 615.1675415039, 312.1889953613, 243.4373779297); + cam2.initPersProjWithoutDistortion(476.0536193848, 476.0534973145, 311.4845581055, 246.2832336426); - dynamic_cast(tracker)->setCameraParameters(cam1, - cam2); + dynamic_cast(tracker)->setCameraParameters(cam1, cam2); tracker->setAngleAppear(vpMath::rad(70)); tracker->setAngleDisappear(vpMath::rad(80)); @@ -455,8 +432,7 @@ int main(int argc, const char **argv) tukey_estimator.MEstimator(residues, weights, 1e-3); for (size_t i = 0; i < weights.size(); i++) { - std::cout << "residues[" << i << "]=" << residues[i] << " ; weights[i" - << i << "]=" << weights[i] << std::endl; + std::cout << "residues[" << i << "]=" << residues[i] << " ; weights[i" << i << "]=" << weights[i] << std::endl; } std::cout << std::endl; } @@ -474,8 +450,7 @@ int main(int argc, const char **argv) tukey_estimator.MEstimator(residues, weights, (float)1e-3); for (size_t i = 0; i < weights.size(); i++) { - std::cout << "residues[" << i << "]=" << residues[i] << " ; weights[i" - << i << "]=" << weights[i] << std::endl; + std::cout << "residues[" << i << "]=" << residues[i] << " ; weights[i" << i << "]=" << weights[i] << std::endl; } std::cout << std::endl; } @@ -512,17 +487,14 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (!getOptions(argc, argv, opt_ipath, opt_configFile, - opt_configFile_depth, opt_modelFile, opt_modelFile_depth, - opt_initFile, displayFeatures, opt_click_allowed, - opt_display, useOgre, showOgreConfigDialog, useScanline, - computeCovariance, projectionError, trackerType_image, - trackerType_depth, opt_lastFrame)) { + if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_configFile_depth, opt_modelFile, opt_modelFile_depth, + opt_initFile, displayFeatures, opt_click_allowed, opt_display, useOgre, showOgreConfigDialog, + useScanline, computeCovariance, projectionError, trackerType_image, trackerType_depth, + opt_lastFrame)) { return EXIT_FAILURE; } -#if !defined(VISP_HAVE_MODULE_KLT) || \ - (!defined(VISP_HAVE_OPENCV) || (VISP_HAVE_OPENCV_VERSION < 0x020100)) +#if !defined(VISP_HAVE_MODULE_KLT) || (!defined(VISP_HAVE_OPENCV) || (VISP_HAVE_OPENCV_VERSION < 0x020100)) if (trackerType_image == /*vpMbGenericTracker::KLT_TRACKER*/ 2) { std::cout << "KLT only features cannot be used: ViSP is not built with " "KLT module or OpenCV is not available." @@ -535,27 +507,20 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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; return EXIT_FAILURE; } // Get the option values - ipath = - vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, - "mbt-depth/castel/castel"); + ipath = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/castel"); - std::string dir_path = vpIoTools::createFilePath( - !opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth"); + std::string dir_path = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth"); if (!vpIoTools::checkDirectory(dir_path)) { - std::cerr << "ViSP-images does not contain the folder: " << dir_path - << "!" << std::endl; + std::cerr << "ViSP-images does not contain the folder: " << dir_path << "!" << std::endl; return EXIT_SUCCESS; } @@ -563,52 +528,40 @@ int main(int argc, const char **argv) if (!opt_configFile.empty()) configFile = opt_configFile; else - configFile = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath - : env_ipath, - "mbt-depth/castel/chateau.xml"); + configFile = + vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.xml"); if (!opt_configFile_depth.empty()) configFile_depth = opt_configFile_depth; else - configFile_depth = vpIoTools::createFilePath( - !opt_ipath.empty() ? opt_ipath : env_ipath, - "mbt-depth/castel/chateau_depth.xml"); + configFile_depth = + vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau_depth.xml"); std::string modelFile, modelFile_depth; if (!opt_modelFile.empty()) modelFile = opt_modelFile; else { -#if defined(VISP_HAVE_COIN3D) && \ - (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || \ - COIN_MAJOR_VERSION == 4) - modelFile = vpIoTools::createFilePath( - !opt_ipath.empty() ? opt_ipath : env_ipath, - "mbt-depth/castel/chateau_gantry.wrl"); +#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4) + modelFile = + vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau_gantry.wrl"); #else - modelFile = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath - : env_ipath, - "mbt-depth/castel/chateau.cao"); + modelFile = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.cao"); #endif } if (!opt_modelFile_depth.empty()) modelFile_depth = opt_modelFile_depth; else - modelFile_depth = vpIoTools::createFilePath( - !opt_ipath.empty() ? opt_ipath : env_ipath, - "mbt-depth/castel/chateau.cao"); + modelFile_depth = + vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.cao"); std::string vrml_ext = ".wrl"; bool use_vrml = - (modelFile.compare(modelFile.length() - vrml_ext.length(), - vrml_ext.length(), vrml_ext) == 0) || - (modelFile_depth.compare(modelFile_depth.length() - vrml_ext.length(), - vrml_ext.length(), vrml_ext) == 0); + (modelFile.compare(modelFile.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0) || + (modelFile_depth.compare(modelFile_depth.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0); if (use_vrml) { -#if defined(VISP_HAVE_COIN3D) && \ - (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || \ - COIN_MAJOR_VERSION == 4) +#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4) std::cout << "use_vrml: " << use_vrml << std::endl; #else std::cerr << "Error: vrml model file is only supported if ViSP is " @@ -621,16 +574,13 @@ int main(int argc, const char **argv) if (!opt_initFile.empty()) initFile = opt_initFile; else - initFile = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath - : env_ipath, - "mbt-depth/castel/chateau.init"); + initFile = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.init"); vpImage I, I_depth; vpImage I_depth_raw; std::vector pointcloud; unsigned int pointcloud_width, pointcloud_height; - if (!read_data(0, ipath, I, I_depth_raw, pointcloud, pointcloud_width, - pointcloud_height)) { + if (!read_data(0, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height)) { std::cerr << "Cannot open sequence: " << ipath << std::endl; return EXIT_FAILURE; } @@ -656,10 +606,8 @@ int main(int argc, const char **argv) display1.setDownScalingFactor(vpDisplay::SCALE_AUTO); display2.setDownScalingFactor(vpDisplay::SCALE_AUTO); display1.init(I, 100, 100, "Test tracking (Left)"); - display2.init(I_depth, - (int)(I.getWidth() / vpDisplay::getDownScalingFactor(I)) + - 110, - 100, "Test tracking (Right)"); + display2.init(I_depth, (int)(I.getWidth() / vpDisplay::getDownScalingFactor(I)) + 110, 100, + "Test tracking (Right)"); #endif vpDisplay::display(I); vpDisplay::display(I_depth); @@ -678,16 +626,13 @@ int main(int argc, const char **argv) vpHomogeneousMatrix depth_M_color; std::string depth_M_color_filename = - vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, - "mbt-depth/castel/depth_M_color.txt"); + vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/depth_M_color.txt"); { std::ifstream depth_M_color_file(depth_M_color_filename.c_str()); depth_M_color.load(depth_M_color_file); - std::map - mapOfCameraTransformationMatrices; + std::map mapOfCameraTransformationMatrices; mapOfCameraTransformationMatrices["Camera2"] = depth_M_color; - dynamic_cast(tracker) - ->setCameraTransformationMatrix(mapOfCameraTransformationMatrices); + dynamic_cast(tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrices); } // Display the moving edges, and the Klt points @@ -708,22 +653,19 @@ int main(int argc, const char **argv) tracker->setProjectionErrorComputation(projectionError); // Retrieve the camera parameters from the tracker - dynamic_cast(tracker)->getCameraParameters(cam1, - cam2); + dynamic_cast(tracker)->getCameraParameters(cam1, cam2); // Loop to position the cube if (opt_display && opt_click_allowed) { while (!vpDisplay::getClick(I, false)) { vpDisplay::display(I); - vpDisplay::displayText( - I, 15, 10, "click after positioning the object", vpColor::red); + vpDisplay::displayText(I, 15, 10, "click after positioning the object", vpColor::red); vpDisplay::flush(I); } } // Load the 3D model (either a vrml file or a .cao file) - dynamic_cast(tracker)->loadModel(modelFile, - modelFile_depth); + dynamic_cast(tracker)->loadModel(modelFile, modelFile_depth); if (opt_display && opt_click_allowed) { std::map *> mapOfImages; @@ -733,34 +675,27 @@ int main(int argc, const char **argv) mapOfInitFiles["Camera1"] = initFile; // Initialise the tracker by clicking on the image - dynamic_cast(tracker)->initClick( - mapOfImages, mapOfInitFiles, true); + dynamic_cast(tracker)->initClick(mapOfImages, mapOfInitFiles, true); dynamic_cast(tracker)->getPose(c1Mo, c2Mo); // display the 3D model at the given pose - dynamic_cast(tracker)->display( - I, I_depth, c1Mo, c2Mo, cam1, cam2, vpColor::red); + dynamic_cast(tracker)->display(I, I_depth, c1Mo, c2Mo, cam1, cam2, vpColor::red); } else { - vpHomogeneousMatrix c1Moi(0.06846423368, 0.09062570884, 0.3401096693, - -2.671882598, 0.1174275908, -0.6011935263); - vpHomogeneousMatrix c2Moi(0.04431452054, 0.09294637757, 0.3357760654, - -2.677922443, 0.121297639, -0.6028463357); - dynamic_cast(tracker)->initFromPose(I, I_depth, - c1Moi, c2Moi); + vpHomogeneousMatrix c1Moi(0.06846423368, 0.09062570884, 0.3401096693, -2.671882598, 0.1174275908, -0.6011935263); + vpHomogeneousMatrix c2Moi(0.04431452054, 0.09294637757, 0.3357760654, -2.677922443, 0.121297639, -0.6028463357); + dynamic_cast(tracker)->initFromPose(I, I_depth, c1Moi, c2Moi); } // track the model { std::map *> mapOfImages; mapOfImages["Camera1"] = &I; - std::map *> - mapOfPointclouds; + std::map *> mapOfPointclouds; mapOfPointclouds["Camera2"] = &pointcloud; std::map mapOfWidths, mapOfHeights; mapOfWidths["Camera2"] = pointcloud_width; mapOfHeights["Camera2"] = pointcloud_height; - dynamic_cast(tracker)->track( - mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights); + dynamic_cast(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights); } dynamic_cast(tracker)->getPose(c1Mo, c2Mo); @@ -772,9 +707,7 @@ int main(int argc, const char **argv) bool quit = false, click = false; unsigned int frame_index = 0; std::vector time_vec; - while (read_data(frame_index, ipath, I, I_depth_raw, pointcloud, - pointcloud_width, pointcloud_height) && - !quit && + while (read_data(frame_index, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height) && !quit && (opt_lastFrame > 0 ? (int)frame_index <= opt_lastFrame : true)) { vpImageConvert::createDepthHistogram(I_depth_raw, I_depth); @@ -798,35 +731,27 @@ int main(int argc, const char **argv) tracker->resetTracker(); loadConfiguration(tracker, configFile, configFile_depth); - dynamic_cast(tracker)->loadModel( - modelFile, modelFile_depth); - dynamic_cast(tracker)->setCameraParameters( - cam1, cam2); + dynamic_cast(tracker)->loadModel(modelFile, modelFile_depth); + dynamic_cast(tracker)->setCameraParameters(cam1, cam2); tracker->setOgreVisibilityTest(useOgre); tracker->setScanLineVisibilityTest(useScanline); tracker->setCovarianceComputation(computeCovariance); tracker->setProjectionErrorComputation(projectionError); - dynamic_cast(tracker)->initFromPose(I, I_depth, - c1Mo, c2Mo); + dynamic_cast(tracker)->initFromPose(I, I_depth, c1Mo, c2Mo); } // Test to set an initial pose #if USE_SMALL_DATASET if (frame_index == 20) { - c1Mo.buildFrom(0.07734634051, 0.08993639906, 0.342344402, - -2.708409543, 0.0669276477, -0.3798958303); - c2Mo.buildFrom(0.05319520317, 0.09223511976, 0.3380095812, - -2.71438192, 0.07141055397, -0.3810081638); + c1Mo.buildFrom(0.07734634051, 0.08993639906, 0.342344402, -2.708409543, 0.0669276477, -0.3798958303); + c2Mo.buildFrom(0.05319520317, 0.09223511976, 0.3380095812, -2.71438192, 0.07141055397, -0.3810081638); #else if (frame_index == 50) { - c1Mo.buildFrom(0.09280663035, 0.09277655672, 0.330415149, - -2.724431817, 0.0293932671, 0.02027966377); - c2Mo.buildFrom(0.06865933578, 0.09494713501, 0.3260555142, - -2.730027451, 0.03498390135, 0.01989831338); + c1Mo.buildFrom(0.09280663035, 0.09277655672, 0.330415149, -2.724431817, 0.0293932671, 0.02027966377); + c2Mo.buildFrom(0.06865933578, 0.09494713501, 0.3260555142, -2.730027451, 0.03498390135, 0.01989831338); #endif std::cout << "Test set pose" << std::endl; - dynamic_cast(tracker)->setPose(I, I_depth, c1Mo, - c2Mo); + dynamic_cast(tracker)->setPose(I, I_depth, c1Mo, c2Mo); } #if USE_SMALL_DATASET @@ -838,16 +763,14 @@ int main(int argc, const char **argv) #endif std::map *> mapOfImages; mapOfImages["Camera1"] = &I; - std::map *> - mapOfPointclouds; + std::map *> mapOfPointclouds; mapOfPointclouds["Camera2"] = &pointcloud; std::map mapOfWidths, mapOfHeights; mapOfWidths["Camera2"] = pointcloud_width; mapOfHeights["Camera2"] = pointcloud_height; double t = vpTime::measureTimeMs(); - dynamic_cast(tracker)->track( - mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights); + dynamic_cast(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights); t = vpTime::measureTimeMs() - t; time_vec.push_back(t); @@ -855,8 +778,7 @@ int main(int argc, const char **argv) if (opt_display) { // display the 3D model - dynamic_cast(tracker)->display( - I, I_depth, c1Mo, c2Mo, cam1, cam2, vpColor::darkRed); + dynamic_cast(tracker)->display(I, I_depth, c1Mo, c2Mo, cam1, cam2, vpColor::darkRed); // display the frame vpDisplay::displayFrame(I, c1Mo, cam1, 0.05); vpDisplay::displayFrame(I_depth, c2Mo, cam2, 0.05); @@ -887,15 +809,11 @@ int main(int argc, const char **argv) } if (computeCovariance) { - std::cout << "Covariance matrix: \n" - << tracker->getCovarianceMatrix() << std::endl - << std::endl; + std::cout << "Covariance matrix: \n" << tracker->getCovarianceMatrix() << std::endl << std::endl; } if (projectionError) { - std::cout << "Projection error: " << tracker->getProjectionError() - << std::endl - << std::endl; + std::cout << "Projection error: " << tracker->getProjectionError() << std::endl << std::endl; } vpDisplay::flush(I); @@ -904,12 +822,9 @@ int main(int argc, const char **argv) frame_index++; } - std::cout << "\nFinal poses, c1Mo:\n" - << c1Mo << "\nc2Mo:\n" - << c2Mo << std::endl; + std::cout << "\nFinal poses, c1Mo:\n" << c1Mo << "\nc2Mo:\n" << c2Mo << std::endl; std::cout << "\nComputation time, Mean: " << vpMath::getMean(time_vec) - << " ms ; Median: " << vpMath::getMedian(time_vec) - << " ms ; Std: " << vpMath::getStdev(time_vec) << " ms" + << " ms ; Median: " << vpMath::getMedian(time_vec) << " ms ; Std: " << vpMath::getStdev(time_vec) << " ms" << std::endl; if (opt_click_allowed && !quit) { @@ -925,9 +840,7 @@ int main(int argc, const char **argv) vpXmlParser::cleanup(); #endif -#if defined(VISP_HAVE_COIN3D) && \ - (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || \ - COIN_MAJOR_VERSION == 4) +#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4) // Cleanup memory allocated by Coin library used to load a vrml model in // vpMbGenericTracker::loadModel() We clean only if Coin was used. if (use_vrml) diff --git a/example/tracking/mbtKltMultiTracking.cpp b/example/tracking/mbtKltMultiTracking.cpp index 4ac4336af5..2b74b8e7ac 100644 --- a/example/tracking/mbtKltMultiTracking.cpp +++ b/example/tracking/mbtKltMultiTracking.cpp @@ -46,9 +46,8 @@ #include #include -#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_MODULE_KLT) && \ - 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 #include @@ -139,12 +138,9 @@ OPTIONS: \n\ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); } -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &configFile, std::string &modelFile, - std::string &initFile, long &lastFrame, - bool &displayKltPoints, bool &click_allowed, bool &display, - bool &cao3DModel, bool &useOgre, bool &showOgreConfigDialog, - bool &useScanline, bool &computeCovariance) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile, + std::string &initFile, long &lastFrame, bool &displayKltPoints, bool &click_allowed, bool &display, + bool &cao3DModel, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, bool &computeCovariance) { const char *optarg_; int c; @@ -245,10 +241,9 @@ int 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, opt_lastFrame, displayKltPoints, - opt_click_allowed, opt_display, cao3DModel, useOgre, - showOgreConfigDialog, useScanline, computeCovariance)) { + if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayKltPoints, + opt_click_allowed, opt_display, cao3DModel, useOgre, showOgreConfigDialog, useScanline, + computeCovariance)) { return (-1); } @@ -256,13 +251,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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; return (-1); } @@ -293,9 +285,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao); #endif } @@ -306,9 +296,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao); #endif } @@ -359,9 +347,7 @@ int main(int argc, const char **argv) display1.setDownScalingFactor(vpDisplay::SCALE_AUTO); display2.setDownScalingFactor(vpDisplay::SCALE_AUTO); display1.init(I1, 100, 100, "Test tracking (Left)"); - display2.init( - I2, (int)I1.getWidth() / vpDisplay::getDownScalingFactor(I1) + 110, - 100, "Test tracking (Right)"); + display2.init(I2, (int)I1.getWidth() / vpDisplay::getDownScalingFactor(I1) + 110, 100, "Test tracking (Right)"); #endif vpDisplay::display(I1); vpDisplay::display(I2); @@ -427,8 +413,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed) { while (!vpDisplay::getClick(I1, false)) { vpDisplay::display(I1); - vpDisplay::displayText( - I1, 15, 10, "click after positioning the object", vpColor::red); + vpDisplay::displayText(I1, 15, 10, "click after positioning the object", vpColor::red); vpDisplay::flush(I1); vpTime::wait(100); } @@ -449,10 +434,8 @@ int main(int argc, const char **argv) // display the 3D model at the given pose tracker.display(I1, I2, c1Mo, c2Mo, cam1, cam2, vpColor::red); } else { - vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, - 2.063603907, 1.110231561, -0.4392789872); - vpHomogeneousMatrix c2Moi(0.02044769891, 0.1101505452, 0.5078963719, - 2.063603907, 1.110231561, -0.4392789872); + vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); + vpHomogeneousMatrix c2Moi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); tracker.initFromPose(I1, I2, c1Moi, c2Moi); } @@ -508,8 +491,7 @@ int main(int argc, const char **argv) // Specify the clipping to use tracker.setNearClippingDistance(0.01); tracker.setFarClippingDistance(0.90); - tracker.setClipping(tracker.getClipping() | - vpMbtPolygon::FOV_CLIPPING); + tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING); // tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING | // vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | // vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING @@ -524,10 +506,8 @@ int main(int argc, const char **argv) // Test to set an initial pose if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) { - c1Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, - 0.8611798108, -0.3491961946); - c2Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, - 0.8611798108, -0.3491961946); + c1Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946); + c2Mo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946); std::cout << "Test set pose" << std::endl; tracker.setPose(I1, I2, c1Mo, c2Mo); } @@ -555,17 +535,14 @@ int main(int argc, const char **argv) } if (computeCovariance) { - std::cout << "Covariance matrix: \n" - << tracker.getCovarianceMatrix() << std::endl - << std::endl; + std::cout << "Covariance matrix: \n" << tracker.getCovarianceMatrix() << std::endl << std::endl; } vpDisplay::flush(I1); vpDisplay::flush(I2); } - std::cout << "Reached last frame: " << reader.getFrameIndex() - << std::endl; + std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl; if (opt_click_allowed && !quit) { vpDisplay::getClick(I1); @@ -579,8 +556,7 @@ int main(int argc, const char **argv) vpXmlParser::cleanup(); #endif -#if defined(VISP_HAVE_COIN3D) && \ - (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3) +#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || 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) diff --git a/example/tracking/mbtKltTracking.cpp b/example/tracking/mbtKltTracking.cpp index 6e6fa269d6..0199f39797 100644 --- a/example/tracking/mbtKltTracking.cpp +++ b/example/tracking/mbtKltTracking.cpp @@ -45,9 +45,8 @@ #include #include -#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_MODULE_KLT) && \ - 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 #include @@ -138,12 +137,9 @@ OPTIONS: \n\ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); } -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &configFile, std::string &modelFile, - std::string &initFile, long &lastFrame, - bool &displayKltPoints, bool &click_allowed, bool &display, - bool &cao3DModel, bool &useOgre, bool &showOgreConfigDialog, - bool &useScanline, bool &computeCovariance) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile, + std::string &initFile, long &lastFrame, bool &displayKltPoints, bool &click_allowed, bool &display, + bool &cao3DModel, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, bool &computeCovariance) { const char *optarg_; int c; @@ -244,10 +240,9 @@ int 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, opt_lastFrame, displayKltPoints, - opt_click_allowed, opt_display, cao3DModel, useOgre, - showOgreConfigDialog, useScanline, computeCovariance)) { + if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayKltPoints, + opt_click_allowed, opt_display, cao3DModel, useOgre, showOgreConfigDialog, useScanline, + computeCovariance)) { return (-1); } @@ -255,13 +250,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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; return (-1); } @@ -292,9 +284,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao); #endif } @@ -305,9 +295,7 @@ int main(int argc, const char **argv) #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; + std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl; modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao); #endif } @@ -416,8 +404,7 @@ int main(int argc, const char **argv) if (opt_display && opt_click_allowed) { while (!vpDisplay::getClick(I, false)) { vpDisplay::display(I); - vpDisplay::displayText( - I, 15, 10, "click after positioning the object", vpColor::red); + vpDisplay::displayText(I, 15, 10, "click after positioning the object", vpColor::red); vpDisplay::flush(I); } } @@ -437,8 +424,7 @@ int main(int argc, const char **argv) // display the 3D model at the given pose tracker.display(I, cMo, cam, vpColor::red); } else { - vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, - 2.063603907, 1.110231561, -0.4392789872); + vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); tracker.initFromPose(I, cMoi); } @@ -486,8 +472,7 @@ int main(int argc, const char **argv) // Specify the clipping to use tracker.setNearClippingDistance(0.01); tracker.setFarClippingDistance(0.90); - tracker.setClipping(tracker.getClipping() | - vpMbtPolygon::FOV_CLIPPING); + tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING); // tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING | // vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | // vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING @@ -502,8 +487,7 @@ int main(int argc, const char **argv) // Test to set an initial pose if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) { - cMo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, - 0.8611798108, -0.3491961946); + 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) { @@ -540,16 +524,13 @@ int main(int argc, const char **argv) } if (computeCovariance) { - std::cout << "Covariance matrix: \n" - << tracker.getCovarianceMatrix() << std::endl - << std::endl; + std::cout << "Covariance matrix: \n" << tracker.getCovarianceMatrix() << std::endl << std::endl; } vpDisplay::flush(I); } - std::cout << "Reached last frame: " << reader.getFrameIndex() - << std::endl; + std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl; if (opt_click_allowed && !quit) { vpDisplay::getClick(I); diff --git a/example/tracking/templateTracker.cpp b/example/tracking/templateTracker.cpp index 2ae899b2e9..efb1072f9a 100644 --- a/example/tracking/templateTracker.cpp +++ b/example/tracking/templateTracker.cpp @@ -117,15 +117,13 @@ typedef enum { #endif -void usage(const char *name, const char *badparam, const WarpType &warp_type, - TrackerType &tracker_type, const long &last_frame); -bool getOptions(int argc, const char **argv, std::string &ipath, - bool &click_allowed, bool &display, bool &pyramidal, - WarpType &warp_type, TrackerType &tracker_type, - long &last_frame); - -void usage(const char *name, const char *badparam, const WarpType &warp_type, - TrackerType &tracker_type, const long &last_frame) +void usage(const char *name, const char *badparam, const WarpType &warp_type, TrackerType &tracker_type, + const long &last_frame); +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, bool &pyramidal, + WarpType &warp_type, TrackerType &tracker_type, long &last_frame); + +void usage(const char *name, const char *badparam, const WarpType &warp_type, TrackerType &tracker_type, + const long &last_frame) { fprintf(stdout, "\n\ Example of template tracking.\n\ @@ -168,8 +166,7 @@ OPTIONS: Default\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, + %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\ @@ -180,8 +177,7 @@ OPTIONS: Default\n\ %d : Homography\n\ %d : Homography in SL3\n\ %d : SRT (scale, rotation, translation)\n\ - %d : Translation\n\n", (int)warp_type, (int)WARP_AFFINE, - (int)WARP_HOMOGRAPHY, (int)WARP_HOMOGRAPHY_SL3, (int)WARP_SRT, + %d : Translation\n\n", (int)warp_type, (int)WARP_AFFINE, (int)WARP_HOMOGRAPHY, (int)WARP_HOMOGRAPHY_SL3, (int)WARP_SRT, (int)WARP_TRANSLATION); #endif @@ -199,14 +195,10 @@ OPTIONS: Default\n\ %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, + %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\ @@ -218,12 +210,9 @@ OPTIONS: Default\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); + %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\ @@ -237,10 +226,8 @@ OPTIONS: Default\n\ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam); } -bool getOptions(int argc, const char **argv, std::string &ipath, - bool &click_allowed, bool &display, bool &pyramidal, - WarpType &warp_type, TrackerType &tracker_type, - long &last_frame) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, bool &pyramidal, + WarpType &warp_type, TrackerType &tracker_type, long &last_frame) { const char *optarg_; int c; @@ -283,16 +270,13 @@ bool getOptions(int argc, const char **argv, std::string &ipath, 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 with \"warp type\"=" - << (int)warp_type << std::endl - << std::endl; + std::cerr << " Bad argument -w with \"warp type\"=" << (int)warp_type << std::endl << std::endl; return false; } 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 with \"tracker type\"=" - << (int)tracker_type << std::endl + std::cerr << " Bad argument -t with \"tracker type\"=" << (int)tracker_type << std::endl << std::endl; return false; } @@ -329,9 +313,8 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (!getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, - opt_pyramidal, opt_warp_type, opt_tracker_type, - opt_last_frame)) { + if (!getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_pyramidal, opt_warp_type, + opt_tracker_type, opt_last_frame)) { return (-1); } @@ -339,13 +322,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, opt_warp_type, opt_tracker_type, opt_last_frame); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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; return (-1); } @@ -489,8 +469,7 @@ int main(int argc, const char **argv) while (!reader.end()) { // Acquire a new image reader.acquire(I); - std::cout << "Process image number " << reader.getFrameIndex() - << std::endl; + std::cout << "Process image number " << reader.getFrameIndex() << std::endl; // Display the image vpDisplay::display(I); // Track the template @@ -505,8 +484,7 @@ int main(int argc, const char **argv) tracker->resetTracker(); if (opt_display && opt_click_allowed) { - vpDisplay::displayText(I, 10, 10, "Re-init simulation", - vpColor::red); + vpDisplay::displayText(I, 10, 10, "Re-init simulation", vpColor::red); vpDisplay::flush(I); tracker->initClick(I, delaunay); } else { diff --git a/example/tracking/trackDot.cpp b/example/tracking/trackDot.cpp index 52194818ac..9db368e263 100644 --- a/example/tracking/trackDot.cpp +++ b/example/tracking/trackDot.cpp @@ -57,9 +57,8 @@ #include #include -#if defined(VISP_HAVE_MODULE_BLOB) && \ - (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 #include @@ -75,11 +74,9 @@ // List of allowed command line options #define GETOPTARGS "cdf:i:n:p:s:h" -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath, unsigned first, unsigned nimages, - unsigned step); -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &ppath, unsigned &first, unsigned &nimages, +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first, + unsigned nimages, unsigned step); +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages, unsigned &step, bool &click_allowed, bool &display); /*! @@ -95,8 +92,8 @@ Print the program options. \param step : Step between two images. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath, unsigned first, unsigned nimages, unsigned step) +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first, + unsigned nimages, unsigned step) { fprintf(stdout, "\n\ Test dot tracking.\n\ @@ -167,8 +164,7 @@ OPTIONS: Default\n\ \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, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages, unsigned &step, bool &click_allowed, bool &display) { const char *optarg_; @@ -244,8 +240,8 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, - opt_step, opt_click_allowed, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_click_allowed, + opt_display) == false) { exit(-1); } @@ -259,27 +255,21 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " 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); + usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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 option if you want to " - << std::endl - << " use personal images." << std::endl - << std::endl; + std::cerr << " Use -i 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 option if you want to " << std::endl + << " use personal images." << std::endl + << std::endl; exit(-1); } @@ -339,8 +329,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -423,14 +412,11 @@ int main(int argc, const char **argv) double time = vpTime::measureTimeMs(); d.track(I); - std::cout << "COG (" << vpTime::measureTimeMs() - time - << " ms): " << std::endl; + std::cout << "COG (" << vpTime::measureTimeMs() - time << " ms): " << std::endl; cog = d.getCog(); - std::cout << cog.get_u() << " " << cog.get_v() << " - " << d.m10 / d.m00 - << " " << d.m01 / d.m00 << std::endl; + std::cout << cog.get_u() << " " << cog.get_v() << " - " << d.m10 / d.m00 << " " << d.m01 / d.m00 << std::endl; std::cout << "Size:" << std::endl; - std::cout << " w: " << d.getWidth() << " h: " << d.getHeight() - << std::endl; + std::cout << " w: " << d.getWidth() << " h: " << d.getHeight() << std::endl; std::cout << "Moments:" << std::endl; std::cout << " m00: " << d.m00 << std::endl; std::cout << " m10: " << d.m10 << std::endl; diff --git a/example/tracking/trackDot2.cpp b/example/tracking/trackDot2.cpp index 583c67e1bb..9ef41d87aa 100644 --- a/example/tracking/trackDot2.cpp +++ b/example/tracking/trackDot2.cpp @@ -50,9 +50,8 @@ #include #include -#if defined(VISP_HAVE_MODULE_BLOB) && \ - (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 #include @@ -70,11 +69,9 @@ // int gsl_warnings_off; -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath, unsigned first, unsigned nimages, - unsigned step); -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &ppath, unsigned &first, unsigned &nimages, +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first, + unsigned nimages, unsigned step); +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages, unsigned &step, bool &click_allowed, bool &display); /*! \example trackDot2.cpp @@ -95,8 +92,8 @@ Print the program options. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath, unsigned first, unsigned nimages, unsigned step) +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first, + unsigned nimages, unsigned step) { fprintf(stdout, "\n\ Test dot tracking using vpDot2 class.\n\ @@ -167,8 +164,7 @@ OPTIONS: Default\n\ \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, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages, unsigned &step, bool &click_allowed, bool &display) { const char *optarg_; @@ -244,8 +240,8 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, - opt_step, opt_click_allowed, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_click_allowed, + opt_display) == false) { exit(-1); } @@ -259,27 +255,21 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " 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); + usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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 option if you want to " - << std::endl - << " use personal images." << std::endl - << std::endl; + std::cerr << " Use -i 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 option if you want to " << std::endl + << " use personal images." << std::endl + << std::endl; exit(-1); } @@ -339,8 +329,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -411,11 +400,10 @@ int main(int argc, const char **argv) if (1) { std::cout << "COG: " << std::endl; cog = d.getCog(); - std::cout << " u: " << cog.get_u() << " v: " << cog.get_v() << " - " - << d.m10 / d.m00 << " " << d.m01 / d.m00 << std::endl; - std::cout << "Size:" << std::endl; - std::cout << " w: " << d.getWidth() << " h: " << d.getHeight() + std::cout << " u: " << cog.get_u() << " v: " << cog.get_v() << " - " << d.m10 / d.m00 << " " << d.m01 / d.m00 << std::endl; + std::cout << "Size:" << std::endl; + std::cout << " w: " << d.getWidth() << " h: " << d.getHeight() << std::endl; std::cout << "Area: " << d.getArea() << std::endl; std::cout << "Moments:" << std::endl; std::cout << " m00: " << d.m00 << std::endl; @@ -432,8 +420,7 @@ int main(int argc, const char **argv) std::cout << " gray level min: " << d.getGrayLevelMin() << std::endl; std::cout << " gray level max: " << d.getGrayLevelMax() << std::endl; std::cout << " size precision: " << d.getSizePrecision() << std::endl; - std::cout << " gray level precision: " << d.getGrayLevelPrecision() - << std::endl; + std::cout << " gray level precision: " << d.getGrayLevelPrecision() << std::endl; } while (iter < opt_first + opt_nimages * opt_step) { @@ -465,14 +452,12 @@ int main(int argc, const char **argv) double time = vpTime::measureTimeMs(); d.track(I); - std::cout << "COG (" << vpTime::measureTimeMs() - time - << " ms): " << std::endl; + std::cout << "COG (" << vpTime::measureTimeMs() - time << " ms): " << std::endl; cog = d.getCog(); - std::cout << " u: " << cog.get_u() << " v: " << cog.get_v() << " - " - << d.m10 / d.m00 << " " << d.m01 / d.m00 << std::endl; - std::cout << "Size:" << std::endl; - std::cout << " w: " << d.getWidth() << " h: " << d.getHeight() + std::cout << " u: " << cog.get_u() << " v: " << cog.get_v() << " - " << d.m10 / d.m00 << " " << d.m01 / d.m00 << std::endl; + std::cout << "Size:" << std::endl; + std::cout << " w: " << d.getWidth() << " h: " << d.getHeight() << std::endl; std::cout << "Area: " << d.getArea() << std::endl; std::cout << "Moments:" << std::endl; std::cout << " m00: " << d.m00 << std::endl; @@ -489,8 +474,7 @@ int main(int argc, const char **argv) std::cout << " gray level min: " << d.getGrayLevelMin() << std::endl; std::cout << " gray level max: " << d.getGrayLevelMax() << std::endl; std::cout << " size precision: " << d.getSizePrecision() << std::endl; - std::cout << " gray level precision: " << d.getGrayLevelPrecision() - << std::endl; + std::cout << " gray level precision: " << d.getGrayLevelPrecision() << std::endl; if (opt_display) { if (0) { diff --git a/example/tracking/trackDot2WithAutoDetection.cpp b/example/tracking/trackDot2WithAutoDetection.cpp index a981d61adc..28c214bd42 100644 --- a/example/tracking/trackDot2WithAutoDetection.cpp +++ b/example/tracking/trackDot2WithAutoDetection.cpp @@ -55,9 +55,8 @@ #include #include -#if defined(VISP_HAVE_MODULE_BLOB) && \ - (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 #include @@ -73,14 +72,11 @@ // List of allowed command line options #define GETOPTARGS "cdi:p:f:n:s:S:G:E:h" -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath, unsigned first, unsigned nimages, unsigned step, - double sizePrecision, double grayLevelPrecision, +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first, + unsigned nimages, unsigned step, double sizePrecision, double grayLevelPrecision, double ellipsoidShapePrecision); -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &ppath, unsigned &first, unsigned &nimages, - unsigned &step, double &sizePrecision, - double &grayLevelPrecision, double &ellipsoidShapePrecision, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages, + unsigned &step, double &sizePrecision, double &grayLevelPrecision, double &ellipsoidShapePrecision, bool &click_allowed, bool &display); /*! @@ -100,9 +96,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath, unsigned first, unsigned nimages, unsigned step, - double sizePrecision, double grayLevelPrecision, +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first, + unsigned nimages, unsigned step, double sizePrecision, double grayLevelPrecision, double ellipsoidShapePrecision) { fprintf(stdout, "\n\ @@ -163,8 +158,8 @@ OPTIONS: Default\n\ 1 means full precision, the shape should be a perfect ellipsoid,\n\ whereas values close to 0 show a very bad precision.\n\ 0 means the shape of dots is not tested \n\ -\n", ipath.c_str(), ppath.c_str(), first, nimages, step, - sizePrecision, grayLevelPrecision, ellipsoidShapePrecision); +\n", ipath.c_str(), ppath.c_str(), first, nimages, step, sizePrecision, grayLevelPrecision, + ellipsoidShapePrecision); fprintf(stdout, "\ -c\n\ @@ -200,10 +195,8 @@ OPTIONS: Default\n\ \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, double &sizePrecision, - double &grayLevelPrecision, double &ellipsoidShapePrecision, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages, + unsigned &step, double &sizePrecision, double &grayLevelPrecision, double &ellipsoidShapePrecision, bool &click_allowed, bool &display) { const char *optarg_; @@ -242,14 +235,14 @@ bool getOptions(int argc, const char **argv, std::string &ipath, ellipsoidShapePrecision = atof(optarg_); break; case 'h': - usage(argv[0], NULL, ipath, ppath, first, nimages, step, sizePrecision, - grayLevelPrecision, ellipsoidShapePrecision); + usage(argv[0], NULL, ipath, ppath, first, nimages, step, sizePrecision, grayLevelPrecision, + ellipsoidShapePrecision); return false; break; default: - usage(argv[0], optarg_, ipath, ppath, first, nimages, step, - sizePrecision, grayLevelPrecision, ellipsoidShapePrecision); + usage(argv[0], optarg_, ipath, ppath, first, nimages, step, sizePrecision, grayLevelPrecision, + ellipsoidShapePrecision); return false; break; } @@ -257,8 +250,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, if ((c == 1) || (c == -1)) { // standalone param or error - usage(argv[0], NULL, ipath, ppath, first, nimages, step, sizePrecision, - grayLevelPrecision, ellipsoidShapePrecision); + usage(argv[0], NULL, ipath, ppath, first, nimages, step, sizePrecision, grayLevelPrecision, + ellipsoidShapePrecision); std::cerr << "ERROR: " << std::endl; std::cerr << " Bad argument " << optarg_ << std::endl << std::endl; return false; @@ -294,10 +287,8 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, - opt_step, opt_sizePrecision, opt_grayLevelPrecision, - opt_ellipsoidShapePrecision, opt_click_allowed, - opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_sizePrecision, + opt_grayLevelPrecision, opt_ellipsoidShapePrecision, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -311,28 +302,22 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " 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_ppath, opt_first, opt_nimages, opt_step, - opt_sizePrecision, opt_grayLevelPrecision, - opt_ellipsoidShapePrecision); + usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_sizePrecision, + opt_grayLevelPrecision, opt_ellipsoidShapePrecision); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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 - << " Use -p option if you want to " - << std::endl - << " use personal images." << std::endl; + std::cerr << " Use -i 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 + << " Use -p option if you want to " << std::endl + << " use personal images." << std::endl; exit(-1); } @@ -390,8 +375,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -425,8 +409,7 @@ int main(int argc, const char **argv) if (opt_click_allowed & opt_display) { d.setGrayLevelPrecision(opt_grayLevelPrecision); - std::cout << "Please click on a dot to initialize detection" - << std::endl; + std::cout << "Please click on a dot to initialize detection" << std::endl; d.initTracking(I); if (opt_display) { @@ -445,8 +428,7 @@ int main(int argc, const char **argv) printf(" gray level max: %u\n", d.getGrayLevelMax()); printf(" grayLevelPrecision: %lf\n", d.getGrayLevelPrecision()); printf(" sizePrecision: %lf\n", d.getSizePrecision()); - printf(" ellipsoidShapePrecision: %lf\n", - d.getEllipsoidShapePrecision()); + printf(" ellipsoidShapePrecision: %lf\n", d.getEllipsoidShapePrecision()); } else { // Set dot characteristics for the auto detection d.setGraphics(true); @@ -488,18 +470,15 @@ int main(int argc, const char **argv) std::cout << "Dot auto detection did not work." << std::endl; return (-1); } else { - std::cout << std::endl - << list_d.size() << " dots are detected" << std::endl; + std::cout << std::endl << list_d.size() << " dots are detected" << std::endl; if (opt_display) { int i = 0; // Parse all founded dots for display - for (std::list::const_iterator it = list_d.begin(); - it != list_d.end(); ++it) { + for (std::list::const_iterator it = list_d.begin(); it != list_d.end(); ++it) { vpImagePoint cog = (*it).getCog(); - std::cout << "Dot " << i++ << " : " << cog.get_u() << " " - << cog.get_v() << std::endl; + std::cout << "Dot " << i++ << " : " << cog.get_u() << " " << cog.get_v() << std::endl; vpDisplay::displayCross(I, cog, 16, vpColor::blue, 3); } diff --git a/example/tracking/trackKltOpencv.cpp b/example/tracking/trackKltOpencv.cpp index 300f74107c..b24d38d9cb 100644 --- a/example/tracking/trackKltOpencv.cpp +++ b/example/tracking/trackKltOpencv.cpp @@ -50,9 +50,8 @@ #include #include -#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_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) @@ -69,11 +68,9 @@ // List of allowed command line options #define GETOPTARGS "cdf:i:n:p:s:h" -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath, unsigned first, unsigned nimages, - unsigned step); -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &ppath, unsigned &first, unsigned &nimages, +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first, + unsigned nimages, unsigned step); +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages, unsigned &step, bool &click_allowed, bool &display); /*! \example trackKltOpencv.cpp @@ -96,8 +93,8 @@ Print the program options. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath, unsigned first, unsigned nimages, unsigned step) +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath, unsigned first, + unsigned nimages, unsigned step) { fprintf(stdout, "\n\ Example of KLT tracking using OpenCV library.\n\ @@ -168,8 +165,7 @@ OPTIONS: Default\n\ \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, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, unsigned &first, unsigned &nimages, unsigned &step, bool &click_allowed, bool &display) { const char *optarg_; @@ -245,8 +241,8 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, - opt_step, opt_click_allowed, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_click_allowed, + opt_display) == false) { exit(-1); } @@ -260,27 +256,21 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " 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); + usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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 option if you want to " - << std::endl - << " use personal images." << std::endl - << std::endl; + std::cerr << " Use -i 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 option if you want to " << std::endl + << " use personal images." << std::endl + << std::endl; exit(-1); } @@ -346,8 +336,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -432,8 +421,7 @@ int main(int argc, const char **argv) double time = vpTime::measureTimeMs(); // Tracking of the detected points tracker.track(cvI); - std::cout << "Tracking performed in " << vpTime::measureTimeMs() - time - << " ms" << std::endl; + std::cout << "Tracking performed in " << vpTime::measureTimeMs() - time << " ms" << std::endl; if (opt_display) { // Display the tracked points diff --git a/example/tracking/trackMeCircle.cpp b/example/tracking/trackMeCircle.cpp index 8358c0b8e8..a22c0d9944 100644 --- a/example/tracking/trackMeCircle.cpp +++ b/example/tracking/trackMeCircle.cpp @@ -55,9 +55,8 @@ #include #include -#if defined(VISP_HAVE_MODULE_ME) && \ - (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 #include @@ -75,8 +74,7 @@ #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); +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display); /*! @@ -131,8 +129,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -191,8 +188,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == - false) { + if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -206,8 +202,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -216,13 +211,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -255,8 +247,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -325,9 +316,7 @@ int main(int argc, const char **argv) std::cout << "A click to exit..." << std::endl; vpDisplay::getClick(I); } - std::cout - << "------------------------------------------------------------" - << std::endl; + std::cout << "------------------------------------------------------------" << std::endl; return 0; } catch (vpException &e) { std::cout << "Catch an exception: " << e << std::endl; diff --git a/example/tracking/trackMeEllipse.cpp b/example/tracking/trackMeEllipse.cpp index 569de6cf31..19e606febd 100644 --- a/example/tracking/trackMeEllipse.cpp +++ b/example/tracking/trackMeEllipse.cpp @@ -56,9 +56,8 @@ #include #include -#if defined(VISP_HAVE_MODULE_ME) && \ - (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 #include @@ -77,8 +76,7 @@ #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); +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display); /*! @@ -133,8 +131,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -193,8 +190,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == - false) { + if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -208,8 +204,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -218,13 +213,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -261,8 +253,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -322,9 +313,7 @@ int main(int argc, const char **argv) std::cout << "A click to continue..." << std::endl; vpDisplay::getClick(I); } - std::cout - << "------------------------------------------------------------" - << std::endl; + std::cout << "------------------------------------------------------------" << std::endl; for (iter = 1; iter < 51; iter++) // initially : iter < 1500 { diff --git a/example/tracking/trackMeLine.cpp b/example/tracking/trackMeLine.cpp index df2284193b..f075d63bec 100644 --- a/example/tracking/trackMeLine.cpp +++ b/example/tracking/trackMeLine.cpp @@ -56,9 +56,8 @@ #include #include -#if defined(VISP_HAVE_MODULE_ME) && \ - (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 #include @@ -81,8 +80,7 @@ #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); +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display); /*! @@ -137,8 +135,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -197,8 +194,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == - false) { + if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -212,8 +208,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -222,13 +217,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -265,8 +257,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -322,17 +313,14 @@ int main(int argc, const char **argv) std::cout << "A click to continue..." << std::endl; vpDisplay::getClick(I); } - std::cout << "----------------------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------------------" << std::endl; vpFeatureLine l; vpCameraParameters cam; vpImage Ic; for (iter = 1; iter < 30; iter++) { - std::cout - << "----------------------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------------------" << std::endl; // set the new image name s.str(""); s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm"; diff --git a/example/tracking/trackMeNurbs.cpp b/example/tracking/trackMeNurbs.cpp index 3758f472d0..1152750aee 100644 --- a/example/tracking/trackMeNurbs.cpp +++ b/example/tracking/trackMeNurbs.cpp @@ -57,9 +57,8 @@ #include #include -#if defined(VISP_HAVE_MODULE_ME) && \ - (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 #include @@ -79,8 +78,7 @@ #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); +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display); /*! @@ -135,8 +133,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -194,8 +191,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == - false) { + if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -209,8 +205,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -219,13 +214,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -280,8 +272,7 @@ int main(int argc, const char **argv) nurbs.setNbControlPoints(14); if (opt_click_allowed) { - std::cout << "Click on points along the edge with the left button." - << std::endl; + std::cout << "Click on points along the edge with the left button." << std::endl; std::cout << "Then click on the right button to continue." << std::endl; nurbs.initTracking(I); } else { @@ -304,9 +295,7 @@ int main(int argc, const char **argv) std::cout << "A click to continue..." << std::endl; vpDisplay::getClick(I); } - std::cout - << "------------------------------------------------------------" - << std::endl; + std::cout << "------------------------------------------------------------" << std::endl; for (int iter = 1; iter < 40; iter++) { // read the image diff --git a/example/video/imageSequenceReader.cpp b/example/video/imageSequenceReader.cpp index 01216847d0..a1faecc6cf 100644 --- a/example/video/imageSequenceReader.cpp +++ b/example/video/imageSequenceReader.cpp @@ -57,16 +57,13 @@ #include #include -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK) +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK) // List of allowed command line options #define GETOPTARGS "cdi:p:f: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, int &first, bool &click_allowed, +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, int &first, bool &click_allowed, bool &display); /*! @@ -79,8 +76,7 @@ Print the program options. \param ppath : Personal image sequence path. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath) +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath) { fprintf(stdout, "\n\ Read an image sequence on the disk.\n\ @@ -138,8 +134,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &ppath, int &first, bool &click_allowed, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, int &first, bool &click_allowed, bool &display) { const char *optarg_; @@ -197,13 +192,11 @@ int main(int argc, const char **argv) bool opt_click_allowed = true; bool opt_display = true; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " videoImageSequenceReader.cpp" << std::endl << std::endl; std::cout << " reading an image sequence" << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH @@ -215,8 +208,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, - opt_click_allowed, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -230,8 +222,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -240,13 +231,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) { usage(argv[0], NULL, ipath, opt_ppath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i option or set VISP_INPUT_IMAGE_PATH " - << std::endl - << " environment variable to specify the location of the " - << std::endl - << " video path where test images are located." << std::endl - << std::endl; + std::cerr << " Use -i option or set VISP_INPUT_IMAGE_PATH " << std::endl + << " environment variable to specify the location of the " << std::endl + << " video path where test images are located." << std::endl + << std::endl; exit(-1); } @@ -269,8 +257,7 @@ int main(int argc, const char **argv) reader.setFileName(filename); reader.setFirstFrameIndex(opt_first); reader.open(I); - std::cout << "Current image number: " << reader.getFrameIndex() - << std::endl; + std::cout << "Current image number: " << reader.getFrameIndex() << std::endl; // We open a window using either X11, GTK, GDI or OpenCV. #if defined VISP_HAVE_X11 @@ -291,14 +278,12 @@ int main(int argc, const char **argv) } if (opt_display && opt_click_allowed) { - std::cout << "Click in the image to read and display the second frame" - << std::endl; + std::cout << "Click in the image to read and display the second frame" << std::endl; vpDisplay::getClick(I); } reader.getFrame(I, opt_first + 1); - std::cout << "Current image number (should be " << opt_first + 1 - << "): " << reader.getFrameIndex() << std::endl; + std::cout << "Current image number (should be " << opt_first + 1 << "): " << reader.getFrameIndex() << std::endl; if (opt_display) { vpDisplay::display(I); @@ -306,14 +291,12 @@ int main(int argc, const char **argv) } if (opt_display && opt_click_allowed) { - std::cout << "Click on the image to read and display the last frame" - << std::endl; + std::cout << "Click on the image to read and display the last frame" << std::endl; vpDisplay::getClick(I); } reader.getFrame(I, reader.getLastFrameIndex()); - std::cout << "Current image number (should be " - << reader.getLastFrameIndex() << "): " << reader.getFrameIndex() + std::cout << "Current image number (should be " << reader.getLastFrameIndex() << "): " << reader.getFrameIndex() << std::endl; if (opt_display) { @@ -330,8 +313,7 @@ int main(int argc, const char **argv) for (int i = opt_first; i <= lastFrame; i++) { reader.getFrame(I, i); - std::cout << "Current image number: " << reader.getFrameIndex() - << std::endl; + std::cout << "Current image number: " << reader.getFrameIndex() << std::endl; if (opt_display) { vpDisplay::display(I); vpDisplay::flush(I); @@ -352,8 +334,7 @@ int main(int argc, const char **argv) #else int main() { - std::cout << "Sorry, no display is available. We quit this example." - << std::endl; + std::cout << "Sorry, no display is available. We quit this example." << std::endl; return 0; } #endif diff --git a/example/video/videoReader.cpp b/example/video/videoReader.cpp index f8e0c00776..52eda695f3 100644 --- a/example/video/videoReader.cpp +++ b/example/video/videoReader.cpp @@ -58,16 +58,14 @@ #include #include -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK) +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK) // 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, bool &click_allowed, bool &display); +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, bool &click_allowed, + bool &display); /*! @@ -79,8 +77,7 @@ Print the program options. \param ppath : Personal video path. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string ppath) +void usage(const char *name, const char *badparam, std::string ipath, std::string ppath) { fprintf(stdout, "\n\ Read a video file on the disk.\n\ @@ -133,8 +130,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &ppath, bool &click_allowed, bool &display) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -187,13 +183,11 @@ int main(int argc, const char **argv) bool opt_click_allowed = true; bool opt_display = true; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << " videoReader.cpp" << std::endl << std::endl; std::cout << " reading a video file" << std::endl; - std::cout << "-------------------------------------------------------" - << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; std::cout << std::endl; // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH @@ -205,8 +199,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed, - opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -220,8 +213,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -230,13 +222,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) { usage(argv[0], NULL, ipath, opt_ppath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i option or set VISP_INPUT_IMAGE_PATH " - << std::endl - << " environment variable to specify the location of the " - << std::endl - << " video path where test images are located." << std::endl - << std::endl; + std::cerr << " Use -i option or set VISP_INPUT_IMAGE_PATH " << std::endl + << " environment variable to specify the location of the " << std::endl + << " video path where test images are located." << std::endl + << std::endl; exit(-1); } @@ -318,8 +307,7 @@ int main(int argc, const char **argv) #else int main() { - std::cout << "Sorry, no display is available. We quit this example." - << std::endl; + std::cout << "Sorry, no display is available. We quit this example." << std::endl; return 0; } #endif diff --git a/example/wireframe-simulator/wireframeSimulator.cpp b/example/wireframe-simulator/wireframeSimulator.cpp index bfa3489bf3..53302bc62b 100644 --- a/example/wireframe-simulator/wireframeSimulator.cpp +++ b/example/wireframe-simulator/wireframeSimulator.cpp @@ -198,13 +198,11 @@ Create a display for each different cameras. // The homogeneous matrix which gives the current position of the main // camera relative to the object - vpHomogeneousMatrix cMo(0, 0.05, 1.3, vpMath::rad(15), vpMath::rad(25), - 0); + vpHomogeneousMatrix cMo(0, 0.05, 1.3, vpMath::rad(15), vpMath::rad(25), 0); // The homogeneous matrix which gives the desired position of the main // camera relative to the object - vpHomogeneousMatrix cdMo(vpHomogeneousMatrix( - 0.0, 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))); // Declaration of the simulator vpWireFrameSimulator sim; @@ -220,8 +218,7 @@ Create a display for each different cameras. Here the object is a plate with 4 points and it is the same object which is used to display the object at the desired position. */ - sim.initScene(vpWireFrameSimulator::PLATE, - vpWireFrameSimulator::D_STANDARD); + sim.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD); /* The object at the current position will be displayed in blue @@ -242,8 +239,7 @@ Create a display for each different cameras. frame. More information about the different frames are given in the html documentation. */ - vpHomogeneousMatrix camMw(vpHomogeneousMatrix(0.0, 0, 4.5, vpMath::rad(0), - vpMath::rad(-30), 0)); + vpHomogeneousMatrix camMw(vpHomogeneousMatrix(0.0, 0, 4.5, vpMath::rad(0), vpMath::rad(-30), 0)); sim.setExternalCameraPosition(camMw); /* @@ -253,8 +249,7 @@ Create a display for each different cameras. sim.setInternalCameraParameters(camera); sim.setExternalCameraParameters(camera); - vpHomogeneousMatrix camoMw(vpHomogeneousMatrix( - -0.3, 0.2, 2.5, vpMath::rad(0), vpMath::rad(10), 0)); + vpHomogeneousMatrix camoMw(vpHomogeneousMatrix(-0.3, 0.2, 2.5, vpMath::rad(0), vpMath::rad(10), 0)); if (opt_display) { // Get the view of the internal camera @@ -317,8 +312,7 @@ Create a display for each different cameras. std::cout << std::endl; if (opt_display) { if (opt_click) { - std::cout << "Click on the internal view window to continue" - << std::endl; + std::cout << "Click on the internal view window to continue" << std::endl; vpDisplay::getClick(Iint); } } @@ -328,8 +322,7 @@ Create a display for each different cameras. "your mouse and move the pointer." << std::endl; std::cout << std::endl; - std::cout << "Click on the internal view window when you are finished" - << std::endl; + std::cout << "Click on the internal view window when you are finished" << std::endl; /* To move the main external camera you need a loop containing the diff --git a/modules/ar/include/visp3/ar/vpAR.h b/modules/ar/include/visp3/ar/vpAR.h index f9f0a57505..4d75cf9949 100644 --- a/modules/ar/include/visp3/ar/vpAR.h +++ b/modules/ar/include/visp3/ar/vpAR.h @@ -125,9 +125,9 @@ int main() vpTime::wait(300); - //Load the cad model. - simu.load("./4points.iv"); //4points.iv can be downloaded on the website -with the image package + // Load the cad model. 4points.iv can be downloaded on the website + // with the image package + simu.load("./4points.iv"); //Initialize the internal camera parameters. simu.setInternalCameraParameters(cam); @@ -151,8 +151,7 @@ class VISP_EXPORT vpAR : public vpSimulator vpAR() : background(false){}; virtual ~vpAR(); - void initInternalViewer(const unsigned int width, const unsigned int height, - vpImageType type = grayImage); + void initInternalViewer(const unsigned int width, const unsigned int height, vpImageType type = grayImage); void setImage(vpImage &I); void setImage(vpImage &I); }; diff --git a/modules/ar/include/visp3/ar/vpAROgre.h b/modules/ar/include/visp3/ar/vpAROgre.h index 3b5e375896..42775a7767 100644 --- a/modules/ar/include/visp3/ar/vpAROgre.h +++ b/modules/ar/include/visp3/ar/vpAROgre.h @@ -95,8 +95,7 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, #endif { public: - vpAROgre(const vpCameraParameters &cam = vpCameraParameters(), - unsigned int width = 0, unsigned int height = 0, + 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, @@ -137,11 +136,9 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, virtual bool customframeEnded(const Ogre::FrameEvent &evt); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMw); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMw); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMw); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMw); inline Ogre::Camera *getCamera() { return mCamera; } @@ -165,10 +162,8 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, inline Ogre::SceneManager *getSceneManager() { return mSceneMgr; } - virtual void init(vpImage &I, bool bufferedKeys = false, - bool hidden = false); - virtual void init(vpImage &I, bool bufferedKeys = false, - bool hidden = false); + virtual void init(vpImage &I, bool bufferedKeys = false, bool hidden = false); + virtual void init(vpImage &I, bool bufferedKeys = false, bool hidden = false); /*! Test if the window is hidden or not. @@ -190,11 +185,9 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, void load(const std::string &entityName, const std::string &model); - bool renderOneFrame(const vpImage &I, - const vpHomogeneousMatrix &cMw); + bool renderOneFrame(const vpImage &I, const vpHomogeneousMatrix &cMw); - bool renderOneFrame(const vpImage &I, - const vpHomogeneousMatrix &cMw); + bool renderOneFrame(const vpImage &I, const vpHomogeneousMatrix &cMw); void setCameraParameters(const vpCameraParameters &cameraP); @@ -231,15 +224,10 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, \param pluginsPath : The new plugins path (must not have a terminate /). */ - inline void setPluginsPath(const char *pluginsPath) - { - mPluginsPath = pluginsPath; - } + 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); + 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. @@ -252,15 +240,11 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, \param resourcePath : The new resource path (must not have a terminate /). */ - inline void setResourcePath(const char *resourcePath) - { - mResourcePath = resourcePath; - } + 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); + 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. @@ -271,10 +255,7 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, \param showConfigDialog : if true, shows the dialog window (used to set the display options) */ - inline void setShowConfigDialog(const bool showConfigDialog) - { - mshowConfigDialog = showConfigDialog; - } + inline void setShowConfigDialog(const bool showConfigDialog) { mshowConfigDialog = showConfigDialog; } void setVisibility(const std::string &sceneName, bool isVisible); @@ -297,12 +278,10 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, 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) + 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"); + throw vpException(vpException::notInitialized, "Window not initialised, cannot set its position"); } mWindow->reposition(static_cast(win_x), static_cast(win_y)); } @@ -333,10 +312,7 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, \return Always true. */ - virtual bool processInputEvent(const Ogre::FrameEvent & /*evt*/) - { - return true; - }; + virtual bool processInputEvent(const Ogre::FrameEvent & /*evt*/) { return true; }; /*! Clean up the 3D scene. @@ -391,19 +367,17 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, #endif // ViSP AR System - bool - keepOn; /** Has the application received a signal to stop(false) or not - (true) */ - vpImage mImageRGBA; /** vpImage to store grabbed image */ - vpImage 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 */ + bool keepOn; /** Has the application received a signal to stop(false) or not + (true) */ + vpImage mImageRGBA; /** vpImage to store grabbed image */ + vpImage 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 **/ @@ -413,10 +387,8 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, bool mshowConfigDialog; /** if true, shows the dialog window (used to set the display options) */ - std:: - list - mOptionnalResourceLocation; /** Optional resource location (used to - load mesh and material) */ + std::list mOptionnalResourceLocation; /** Optional resource location (used to + load mesh and material) */ }; #endif // VISP_HAVE_OGRE diff --git a/modules/ar/include/visp3/ar/vpSimulator.h b/modules/ar/include/visp3/ar/vpSimulator.h index 613b5b0f23..b5d6b1a038 100644 --- a/modules/ar/include/visp3/ar/vpSimulator.h +++ b/modules/ar/include/visp3/ar/vpSimulator.h @@ -140,8 +140,7 @@ class VISP_EXPORT vpSimulator public: //! initialize the camera view - virtual void initInternalViewer(const unsigned int nlig, - const unsigned int ncol); + 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); @@ -207,8 +206,7 @@ class VISP_EXPORT vpSimulator void initSceneGraph(); //! Add a new object in the scene graph ad a given location - void addObject(SoSeparator *object, const vpHomogeneousMatrix &fMo, - SoSeparator *root); + void addObject(SoSeparator *object, const vpHomogeneousMatrix &fMo, SoSeparator *root); public: //! Add a new object in the scene graph ad a given location @@ -274,8 +272,7 @@ class VISP_EXPORT vpSimulator protected: SbTime *realtime; SoOffscreenRenderer *offScreenRenderer; - void offScreenRendering(vpSimulatorViewType view = vpSimulator::EXTERNAL, - int *width = NULL, int *height = NULL); + void offScreenRendering(vpSimulatorViewType view = vpSimulator::EXTERNAL, int *width = NULL, int *height = NULL); public: //! image of the internal view @@ -293,10 +290,7 @@ class VISP_EXPORT vpSimulator void getSizeInternalView(int &width, int &height); //! get the intrinsic parameters of the camera - void getCameraParameters(vpCameraParameters &cam) - { - cam = internalCameraParameters; - } + void getCameraParameters(vpCameraParameters &cam) { cam = internalCameraParameters; } }; #endif diff --git a/modules/ar/src/coin-simulator/vpAR.cpp b/modules/ar/src/coin-simulator/vpAR.cpp index 6f2c69668f..4b584f0ee8 100644 --- a/modules/ar/src/coin-simulator/vpAR.cpp +++ b/modules/ar/src/coin-simulator/vpAR.cpp @@ -52,21 +52,21 @@ #include /* Objets OIV. */ -#include /* Objet cone. */ -#include /* Liste de points. */ -#include /* Objet cylindre. */ +#include /* Objet cone. */ +#include /* Liste de points. */ +#include /* Objet cylindre. */ #include /* Liste de face. */ -#include /* Objet lumiere ponctuelle. */ -#include /* Transfo rotation simple. */ -#include /* Trasnfo mise a l'echelle. */ -#include /* Trasnfo translation. */ +#include /* Objet lumiere ponctuelle. */ +#include /* Transfo rotation simple. */ +#include /* Trasnfo mise a l'echelle. */ +#include /* Trasnfo translation. */ #include #include /* Objet lumiere directionnelle*/ -#include /* Style de rendu. */ -#include /* Eclairage ambiant. */ -#include /* Groupement de noeuds (sans separation)*/ -#include /* Matiere (couleur) des objets. */ +#include /* Style de rendu. */ +#include /* Eclairage ambiant. */ +#include /* Groupement de noeuds (sans separation)*/ +#include /* Matiere (couleur) des objets. */ /*! Basic Destructor that calls the kill() method of the vpSimulator @@ -81,8 +81,7 @@ vpAR::~vpAR() { kill(); } \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) +void vpAR::initInternalViewer(const unsigned int width, const unsigned int height, vpImageType type) { vpSimulator::initInternalViewer(width, height); @@ -97,11 +96,9 @@ void vpAR::initInternalViewer(const unsigned int width, typeImage = type; if (typeImage == grayImage) - image_background = - (GLubyte *)malloc(internal_width * internal_height * sizeof(GLubyte)); + image_background = (GLubyte *)malloc(internal_width * internal_height * sizeof(GLubyte)); else - image_background = (GLubyte *)malloc(3 * internal_width * - internal_height * sizeof(GLubyte)); + image_background = (GLubyte *)malloc(3 * internal_width * internal_height * sizeof(GLubyte)); } /*! @@ -113,11 +110,9 @@ void vpAR::initInternalViewer(const unsigned int width, void vpAR::setImage(vpImage &I) { - if ((internal_width != I.getWidth()) || - (internal_height != I.getHeight())) { + 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"); + throw(vpException(vpException::dimensionError), "The image size is different from the view size"); } background = true; @@ -138,11 +133,9 @@ void vpAR::setImage(vpImage &I) void vpAR::setImage(vpImage &I) { - if ((internal_width != I.getWidth()) || - (internal_height != I.getHeight())) { + 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"); + throw(vpException(vpException::dimensionError), "The image size is different from the view size"); } background = true; @@ -152,12 +145,9 @@ void vpAR::setImage(vpImage &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() * 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; + 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; } } diff --git a/modules/ar/src/coin-simulator/vpSimulator.cpp b/modules/ar/src/coin-simulator/vpSimulator.cpp index 28a3d91947..aa2a635a0c 100644 --- a/modules/ar/src/coin-simulator/vpSimulator.cpp +++ b/modules/ar/src/coin-simulator/vpSimulator.cpp @@ -56,21 +56,21 @@ #endif /* Objets OIV. */ -#include /* Objet cone. */ -#include /* Liste de points. */ -#include /* Objet cylindre. */ +#include /* Objet cone. */ +#include /* Liste de points. */ +#include /* Objet cylindre. */ #include /* Liste de face. */ -#include /* Objet lumiere ponctuelle. */ -#include /* Transfo rotation simple. */ -#include /* Trasnfo mise a l'echelle. */ -#include /* Trasnfo translation. */ +#include /* Objet lumiere ponctuelle. */ +#include /* Transfo rotation simple. */ +#include /* Trasnfo mise a l'echelle. */ +#include /* Trasnfo translation. */ #include #include /* Objet lumiere directionnelle*/ -#include /* Style de rendu. */ -#include /* Eclairage ambiant. */ -#include /* Groupement de noeuds (sans separation)*/ -#include /* Matiere (couleur) des objets. */ +#include /* Style de rendu. */ +#include /* Eclairage ambiant. */ +#include /* Groupement de noeuds (sans separation)*/ +#include /* Matiere (couleur) des objets. */ // Positions of all of the vertices: // @@ -132,8 +132,7 @@ SoSeparator *makePyramide() * est , et celui de la fleche * 5. * La fleche est oriente selon l'axe Y. */ -static SoSeparator *createArrow(float longueur, float proportionFleche, - float radius) +static SoSeparator *createArrow(float longueur, float proportionFleche, float radius) { SoSeparator *fleche = new SoSeparator; fleche->ref(); @@ -173,8 +172,7 @@ static SoSeparator *createArrow(float longueur, float proportionFleche, #define RAYON_FLECHE 0.002f #define PROPORTION_FLECHE 0.1f -SoSeparator *createFrame(float longueurFleche = LONGUEUR_FLECHE, - float proportionFleche = PROPORTION_FLECHE, +SoSeparator *createFrame(float longueurFleche = LONGUEUR_FLECHE, float proportionFleche = PROPORTION_FLECHE, float radiusFleche = RAYON_FLECHE) { vpDEBUG_TRACE(15, "# Entree."); @@ -206,8 +204,7 @@ SoSeparator *createFrame(float longueurFleche = LONGUEUR_FLECHE, bleu->diffuseColor.setValue(0.0, 0.0, 1.0); bleu->emissiveColor.setValue(0.0, 0.0, 0.5); - SoSeparator *fleche = - createArrow(longueurFleche, proportionFleche, radiusFleche); + SoSeparator *fleche = createArrow(longueurFleche, proportionFleche, radiusFleche); frame->addChild(rouge); frame->addChild(rotationY_X); @@ -335,16 +332,12 @@ vpSimulator::vpSimulator() #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) + 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(); } @@ -454,8 +447,7 @@ void vpSimulator::changeZoomFactor(const float zoomFactor, const int index) // this->setZoomFactor(zoomFactor); } -void vpSimulator::initInternalViewer(const unsigned int width, - const unsigned int height) +void vpSimulator::initInternalViewer(const unsigned int width, const unsigned int height) { internal_width = width; internal_height = height; @@ -489,8 +481,7 @@ void vpSimulator::initInternalViewer(const unsigned int width, bufferView = new unsigned char[3 * width * height]; } -void vpSimulator::initExternalViewer(const unsigned int width, - const unsigned int height) +void vpSimulator::initExternalViewer(const unsigned int width, const unsigned int height) { external_width = width; @@ -527,8 +518,7 @@ void vpSimulator::setInternalCameraParameters(vpCameraParameters &_cam) internalCamera->ref(); internalCamera->heightAngle = 2 * atan(v); - internalCamera->aspectRatio = - (internal_width / internal_height) * (px / py); + internalCamera->aspectRatio = (internal_width / internal_height) * (px / py); internalCamera->nearDistance = 0.001f; internalCamera->farDistance = 1000; @@ -547,8 +537,7 @@ void vpSimulator::setExternalCameraParameters(vpCameraParameters &_cam) externalCamera->ref(); externalCamera->heightAngle = 2 * atan(v); - externalCamera->aspectRatio = - (external_width / external_height) * (px / py); + externalCamera->aspectRatio = (external_width / external_height) * (px / py); externalCamera->nearDistance = 0.001f; externalCamera->farDistance = 1000; externalCamera->unrefNoDelete(); @@ -618,8 +607,7 @@ void vpSimulator::moveInternalCamera(vpHomogeneousMatrix &cMf) rotCam.setValue(matrix); internalCameraPosition->ref(); internalCameraPosition->rotation.setValue(rotCam); - internalCameraPosition->translation.setValue(matrix[3][0], matrix[3][1], - matrix[3][2]); + internalCameraPosition->translation.setValue(matrix[3][0], matrix[3][1], matrix[3][2]); internalCameraPosition->unref(); } @@ -726,8 +714,7 @@ void vpSimulator::addFrame(const vpHomogeneousMatrix &fMo, float zoom) SoSeparator *frame = new SoSeparator; frame->ref(); frame->addChild(taille); - frame->addChild(createFrame(LONGUEUR_FLECHE * zoom, - PROPORTION_FLECHE * zoom, RAYON_FLECHE * zoom)); + frame->addChild(createFrame(LONGUEUR_FLECHE * zoom, PROPORTION_FLECHE * zoom, RAYON_FLECHE * zoom)); this->addObject(frame, fMo, externalRoot); // frame->unref(); } @@ -739,8 +726,7 @@ void vpSimulator::addFrame(const vpHomogeneousMatrix &fMo, float zoom) */ void vpSimulator::addAbsoluteFrame(float zoom) { - scene->addChild(createFrame(LONGUEUR_FLECHE * zoom, - PROPORTION_FLECHE * zoom, RAYON_FLECHE * zoom)); + scene->addChild(createFrame(LONGUEUR_FLECHE * zoom, PROPORTION_FLECHE * zoom, RAYON_FLECHE * zoom)); } /*! @@ -748,8 +734,7 @@ void vpSimulator::addAbsoluteFrame(float zoom) \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) +void vpSimulator::load(const char *iv_filename, const vpHomogeneousMatrix &fMo) { SoInput in; @@ -777,8 +762,7 @@ void vpSimulator::load(const char *iv_filename, \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) +void vpSimulator::addObject(SoSeparator *newObject, const vpHomogeneousMatrix &fMo) { try { this->addObject(newObject, fMo, scene); @@ -795,8 +779,7 @@ void vpSimulator::addObject(SoSeparator *newObject, \param root : pointer toward the subscene graph */ -void vpSimulator::addObject(SoSeparator *object, - const vpHomogeneousMatrix &fMo, SoSeparator *root) +void vpSimulator::addObject(SoSeparator *object, const vpHomogeneousMatrix &fMo, SoSeparator *root) { bool identity = true; @@ -828,8 +811,7 @@ void vpSimulator::addObject(SoSeparator *object, SoSeparator *newNode = new SoSeparator; displacement->rotation.setValue(rotation); - displacement->translation.setValue(matrix[3][0], matrix[3][1], - matrix[3][2]); + displacement->translation.setValue(matrix[3][0], matrix[3][1], matrix[3][2]); root->addChild(newNode); newNode->addChild(displacement); @@ -884,8 +866,7 @@ void vpSimulator::closeMainApplication() * - 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) +void vpSimulator::offScreenRendering(vpSimulatorViewType view, int *width, int *height) { SbVec2s size(320, 200); @@ -967,8 +948,7 @@ void vpSimulator::write(const char *fileName) 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); + unsigned int index = 3 * ((internal_height - i - 1) * internal_width + j); r = *(bufferView + index); g = *(bufferView + index + 1); b = *(bufferView + index + 2); @@ -999,8 +979,7 @@ void vpSimulator::getInternalImage(vpImage &I) // while (get==0) {;} get = 2; I.resize(internal_height, internal_width); - vpImageConvert::RGBToRGBa(bufferView, (unsigned char *)I.bitmap, - internal_width, internal_height, true); + vpImageConvert::RGBToRGBa(bufferView, (unsigned char *)I.bitmap, internal_width, internal_height, true); get = 1; } @@ -1013,8 +992,7 @@ void vpSimulator::getInternalImage(vpImage &I) // while (get==0) {;} get = 2; I.resize(internal_height, internal_width); - vpImageConvert::RGBToGrey(bufferView, I.bitmap, internal_width, - internal_height, true); + vpImageConvert::RGBToGrey(bufferView, I.bitmap, internal_width, internal_height, true); get = 1; } diff --git a/modules/ar/src/coin-simulator/vpViewer.cpp b/modules/ar/src/coin-simulator/vpViewer.cpp index ddcd50904f..7316886e58 100644 --- a/modules/ar/src/coin-simulator/vpViewer.cpp +++ b/modules/ar/src/coin-simulator/vpViewer.cpp @@ -91,12 +91,10 @@ void vpViewer::actualRedraw(void) 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, + 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, + glDrawPixels((GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(), (GLenum)GL_RGB, GL_UNSIGNED_BYTE, simu->image_background); glEnable(GL_DEPTH_TEST); @@ -114,8 +112,7 @@ void vpViewer::actualRedraw(void) glSwapBuffers(); if (viewerType == vpViewer::internalView) { simu->get = 0; - glReadPixels(0, 0, (GLsizei)simu->getInternalWidth(), - (GLsizei)simu->getInternalHeight(), (GLenum)GL_RGB, + glReadPixels(0, 0, (GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(), (GLenum)GL_RGB, GL_UNSIGNED_BYTE, simu->bufferView); simu->get = 1; } @@ -149,8 +146,7 @@ vpViewer::resize(int x, int y, bool /*fixed*/) 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); + MoveWindow(parent, rcWindow.left, rcWindow.top, x + ptDiff.x, y + ptDiff.y, TRUE); if (fixed) { DWORD dwStyle = GetWindowLong(parent, GWL_STYLE); dwStyle &= ~(WS_SIZEBOX); @@ -174,21 +170,16 @@ vpViewer::resize(int x, int y, bool /*fixed*/) SbBool vpViewer::processSoEvent(const SoEvent *const event) { - if (this->isViewing() && - event->getTypeId() == SoKeyboardEvent::getClassTypeId()) { + 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 : 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; + std::cout << "P : get external camera location and set the internal one" << std::endl; } return TRUE; diff --git a/modules/ar/src/ogre-simulator/vpAROgre.cpp b/modules/ar/src/ogre-simulator/vpAROgre.cpp index 93caaeed63..48c1fe9cdc 100644 --- a/modules/ar/src/ogre-simulator/vpAROgre.cpp +++ b/modules/ar/src/ogre-simulator/vpAROgre.cpp @@ -70,20 +70,17 @@ telling Ogre where to look for renderer plugins. */ -vpAROgre::vpAROgre(const vpCameraParameters &cam, unsigned int width, - unsigned int height, const char *resourcePath, +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), + : 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(), mBackground(NULL), - mBackgroundHeight(0), mBackgroundWidth(0), mWindowHeight(height), - mWindowWidth(width), windowHidden(false), mNearClipping(0.001), - mFarClipping(200), mcam(cam), mshowConfigDialog(true), - mOptionnalResourceLocation() + mImageRGBA(), mImage(), mPixelBuffer(), mBackground(NULL), mBackgroundHeight(0), mBackgroundWidth(0), + mWindowHeight(height), mWindowWidth(width), windowHidden(false), mNearClipping(0.001), mFarClipping(200), mcam(cam), + mshowConfigDialog(true), mOptionnalResourceLocation() { } @@ -222,8 +219,7 @@ void vpAROgre::init(bool // mPluginsPath may contain more than one folder location separated by ";" bool pluginsFileExists = false; std::string pluginFile; - std::vector plugingsPaths = - vpIoTools::splitChain(std::string(mPluginsPath), std::string(";")); + std::vector 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"; @@ -243,14 +239,12 @@ void vpAROgre::init(bool #else + std::string("plugins_d.cfg") #endif - + std::string("\" doesn't exist in ") + - std::string(mPluginsPath); + + 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; + std::cout << "######################### Load plugin file: " << pluginFile << std::endl; if (Ogre::Root::getSingletonPtr() == NULL) mRoot = new Ogre::Root(pluginFile, "ogre.cfg", "Ogre.log"); @@ -271,8 +265,7 @@ void vpAROgre::init(bool // mResourcePath may contain more than one folder location separated by ";" bool resourcesFileExists = false; std::string resourceFile; - std::vector resourcesPaths = - vpIoTools::splitChain(std::string(mResourcePath), std::string(";")); + std::vector 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)) { @@ -281,16 +274,14 @@ void vpAROgre::init(bool } } if (!resourcesFileExists) { - std::string errorMsg = - std::string("Error: the requested resource file \"resources.cfg\"") + - std::string("doesn't exist in ") + std::string(mResourcePath); + 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; + std::cout << "######################### Load resource file: " << resourceFile << std::endl; Ogre::ConfigFile cf; cf.load(resourceFile); @@ -305,18 +296,15 @@ void vpAROgre::init(bool for (i = settings->begin(); i != settings->end(); ++i) { typeName = i->first; archName = i->second; - Ogre::ResourceGroupManager::getSingleton().addResourceLocation( - archName, typeName, secName); + Ogre::ResourceGroupManager::getSingleton().addResourceLocation(archName, typeName, secName); } } std::cout << "##################### add resources" << std::endl; // Add optionnal resources (given by the user). - for (std::list::const_iterator iter = - mOptionnalResourceLocation.begin(); + for (std::list::const_iterator iter = mOptionnalResourceLocation.begin(); iter != mOptionnalResourceLocation.end(); ++iter) { Ogre::ResourceGroupManager::getSingleton().addResourceLocation( - *iter, "FileSystem", - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + *iter, "FileSystem", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); } // Create the window @@ -332,14 +320,12 @@ void vpAROgre::init(bool if (!mRoot->isInitialised()) { if (!canInit) { // We set the default renderer system - const Ogre::RenderSystemList &lRenderSystemList = - mRoot->getAvailableRenderers(); + 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; + std::cout << "Using " << lRenderSystem->getName() << " as renderer." << std::endl; mRoot->setRenderSystem(lRenderSystem); } @@ -386,8 +372,7 @@ void vpAROgre::init(bool windowHidden = true; #endif } - mWindow = mRoot->createRenderWindow(name, mWindowWidth, mWindowHeight, - fullscreen, &misc); + mWindow = mRoot->createRenderWindow(name, mWindowWidth, mWindowHeight, fullscreen, &misc); // Initialise resources Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups(); @@ -434,33 +419,29 @@ void vpAROgre::init(bool 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"))); +#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( - mInputManager->createInputObject(OIS::OISKeyboard, bufferedKeys)); + mKeyboard = static_cast(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, 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); + // 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); @@ -483,9 +464,7 @@ vpAROgre::~vpAROgre(void) } // Delete root - if (Ogre::Root::getSingletonPtr() && !Ogre::Root::getSingletonPtr() - ->getSceneManagerIterator() - .hasMoreElements()) { + if (Ogre::Root::getSingletonPtr() && !Ogre::Root::getSingletonPtr()->getSceneManagerIterator().hasMoreElements()) { if (mRoot) { delete mRoot; } @@ -578,10 +557,7 @@ bool vpAROgre::customframeStarted(const Ogre::FrameEvent & /*evt*/) \param evt : Frame event to process. \return True if everything went well. */ -bool vpAROgre::customframeEnded(const Ogre::FrameEvent & /*evt*/) -{ - return true; -} +bool vpAROgre::customframeEnded(const Ogre::FrameEvent & /*evt*/) { return true; } /*! @@ -605,8 +581,7 @@ void vpAROgre::windowClosed(Ogre::RenderWindow *rw) \param I : Grey level image to show in background. \param cMw : Camera pose as an homogeneous matrix. */ -bool vpAROgre::renderOneFrame(const vpImage &I, - const vpHomogeneousMatrix &cMw) +bool vpAROgre::renderOneFrame(const vpImage &I, const vpHomogeneousMatrix &cMw) { // Update the background to match the situation updateBackgroundTexture(I); @@ -623,8 +598,7 @@ bool vpAROgre::renderOneFrame(const vpImage &I, \param I : RGBa image to show in background. \param cMw : Camera pose as an homogeneous matrix. */ -bool vpAROgre::renderOneFrame(const vpImage &I, - const vpHomogeneousMatrix &cMw) +bool vpAROgre::renderOneFrame(const vpImage &I, const vpHomogeneousMatrix &cMw) { // Update the background to match the situation updateBackgroundTexture(I); @@ -641,8 +615,7 @@ bool vpAROgre::renderOneFrame(const vpImage &I, \param I : Grey level image to show in background. \param cMw : Camera pose as an homogeneous matrix. */ -void vpAROgre::display(const vpImage &I, - const vpHomogeneousMatrix &cMw) +void vpAROgre::display(const vpImage &I, const vpHomogeneousMatrix &cMw) { // Display on Ogre Window if (renderOneFrame(I, cMw)) { @@ -657,8 +630,7 @@ void vpAROgre::display(const vpImage &I, \param I : RGBa image to show in background. \param cMw : Camera pose as an homogeneous matrix. */ -void vpAROgre::display(const vpImage &I, - const vpHomogeneousMatrix &cMw) +void vpAROgre::display(const vpImage &I, const vpHomogeneousMatrix &cMw) { // Display on Ogre Window if (renderOneFrame(I, cMw)) { @@ -677,10 +649,7 @@ bool vpAROgre::continueRendering(void) { return keepOn; } /*! Set the camera intrinsic parameters */ -void vpAROgre::setCameraParameters(const vpCameraParameters &cameraP) -{ - mcam = cameraP; -} +void vpAROgre::setCameraParameters(const vpCameraParameters &cameraP) { mcam = cameraP; } /*! Load a mesh in the 3D world. @@ -690,8 +659,7 @@ void vpAROgre::setCameraParameters(const vpCameraParameters &cameraP) 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); + Ogre::SceneNode *newNode = mSceneMgr->getRootSceneNode()->createChildSceneNode(entityName); newNode->attachObject(newEntity); } @@ -701,13 +669,11 @@ void vpAROgre::load(const std::string &entityName, const std::string &model) \param wTo : New position of the node (translation between object frame and world frame). */ -void vpAROgre::setPosition(const std::string &sceneName, - const vpTranslationVector &wTo) +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]); + node->setPosition((Ogre::Real)wTo[0], (Ogre::Real)wTo[1], (Ogre::Real)wTo[2]); } /*! @@ -717,11 +683,8 @@ void vpAROgre::setPosition(const std::string &sceneName, */ 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]); + Ogre::Vector3 translation = mSceneMgr->getSceneNode(sceneName)->getPosition(); + return vpTranslationVector((Ogre::Real)translation[0], (Ogre::Real)translation[1], (Ogre::Real)translation[2]); } /*! @@ -729,16 +692,14 @@ vpTranslationVector vpAROgre::getPosition(const std::string &sceneName) const \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) +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::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); } @@ -748,14 +709,12 @@ void vpAROgre::setRotation(const std::string &sceneName, \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) +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::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); } @@ -768,8 +727,7 @@ void vpAROgre::addRotation(const std::string &sceneName, translation to apply. */ -void vpAROgre::setPosition(const std::string &sceneName, - const vpHomogeneousMatrix &wMo) +void vpAROgre::setPosition(const std::string &sceneName, const vpHomogeneousMatrix &wMo) { // Extract the position and orientation data vpRotationMatrix rotations; @@ -798,25 +756,18 @@ void vpAROgre::setVisibility(const std::string &sceneName, bool isVisible) \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) +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()); + 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)); + mSceneMgr->getSceneNode(sceneName)->scale(Ogre::Vector3(factorx, factory, factorz)); } /*! Create the Ogre camera. */ -void vpAROgre::createCamera(void) -{ - mCamera = mSceneMgr->createCamera("Camera"); -} +void vpAROgre::createCamera(void) { mCamera = mSceneMgr->createCamera("Camera"); } /*! Create a greylevel background to show the real scene. @@ -829,13 +780,11 @@ void vpAROgre::createBackground(vpImage & /* 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 + 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().setDefaultTextureFiltering(Ogre::TFO_NONE); Ogre::MaterialManager::getSingleton().setDefaultAnisotropy(1); // Dynamic texture @@ -843,18 +792,14 @@ void vpAROgre::createBackground(vpImage & /* I */) // dynamic texture if (mRoot->getRenderSystem()->getName() == "OpenGL Rendering Subsystem") { Ogre::TextureManager::getSingleton().createManual( - "BackgroundTexture", - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - Ogre::TEX_TYPE_2D, + "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, + "BackgroundTexture", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, mBackgroundWidth, // width mBackgroundHeight, // height 0, // num of mip maps @@ -862,8 +807,7 @@ void vpAROgre::createBackground(vpImage & /* I */) } // Pointer to the dynamic texture - Ogre::TexturePtr dynTexPtr = - Ogre::TextureManager::getSingleton().getByName("BackgroundTexture"); + Ogre::TexturePtr dynTexPtr = Ogre::TextureManager::getSingleton().getByName("BackgroundTexture"); //#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) ) // .dynamicCast();// Get the pixel buffer //#else @@ -872,33 +816,24 @@ void vpAROgre::createBackground(vpImage & /* I */) 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); + Ogre::MaterialPtr Backgroundmaterial = Ogre::MaterialManager::getSingleton().create( + "BackgroundMaterial", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); //#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) ) // .dynamicCast(); //#else // ; //#endif - Ogre::Technique *Backgroundtechnique = - Backgroundmaterial->createTechnique(); + 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 + 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"); + Ogre::SceneNode *BackgroundNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("BackgoundNode"); BackgroundNode->attachObject(mBackground); } @@ -913,13 +848,11 @@ void vpAROgre::createBackground(vpImage & /* 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 + 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().setDefaultTextureFiltering(Ogre::TFO_NONE); Ogre::MaterialManager::getSingleton().setDefaultAnisotropy(1); // Dynamic texture @@ -927,9 +860,7 @@ void vpAROgre::createBackground(vpImage & /* I */) // dynamic texture if (mRoot->getRenderSystem()->getName() == "OpenGL Rendering Subsystem") { Ogre::TextureManager::getSingleton().createManual( - "BackgroundTexture", - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - Ogre::TEX_TYPE_2D, + "BackgroundTexture", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, mBackgroundWidth, // width mBackgroundHeight, // height 0, // num of mip maps @@ -938,9 +869,7 @@ void vpAROgre::createBackground(vpImage & /* I */) } 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, + "BackgroundTexture", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, mBackgroundWidth, // width mBackgroundHeight, // height 0, // num of mip maps @@ -949,8 +878,7 @@ void vpAROgre::createBackground(vpImage & /* I */) } // Pointer to the dynamic texture - Ogre::TexturePtr dynTexPtr = - Ogre::TextureManager::getSingleton().getByName("BackgroundTexture"); + Ogre::TexturePtr dynTexPtr = Ogre::TextureManager::getSingleton().getByName("BackgroundTexture"); //#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) ) // .dynamicCast();// Get the pixel buffer //#else @@ -961,33 +889,24 @@ void vpAROgre::createBackground(vpImage & /* I */) 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); + Ogre::MaterialPtr Backgroundmaterial = Ogre::MaterialManager::getSingleton().create( + "BackgroundMaterial", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); //#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) ) // .dynamicCast(); //#else // ; //#endif - Ogre::Technique *Backgroundtechnique = - Backgroundmaterial->createTechnique(); + 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 + 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"); + Ogre::SceneNode *BackgroundNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("BackgoundNode"); BackgroundNode->attachObject(mBackground); } @@ -1027,13 +946,10 @@ void vpAROgre::updateCameraProjection(void) 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); + 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); } } @@ -1088,8 +1004,7 @@ void vpAROgre::updateBackgroundTexture(const vpImage &I) } } #else // if texture in RGBa format which is the format of the input image - memcpy(pDest, I.bitmap, - mBackgroundHeight * mBackgroundWidth * sizeof(vpRGBa)); + memcpy(pDest, I.bitmap, mBackgroundHeight * mBackgroundWidth * sizeof(vpRGBa)); #endif // Unlock the pixel buffer @@ -1106,14 +1021,10 @@ void vpAROgre::updateCameraParameters(const vpHomogeneousMatrix &cMw) 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); + = 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); } @@ -1123,12 +1034,10 @@ void vpAROgre::updateCameraParameters(const vpHomogeneousMatrix &cMw) \param I : The image on which to copy the result of the rendering loop. \param cMo : The desired camera pose. */ -void vpAROgre::getRenderingOutput(vpImage &I, - const vpHomogeneousMatrix &cMo) +void vpAROgre::getRenderingOutput(vpImage &I, const vpHomogeneousMatrix &cMo) { updateCameraParameters(cMo); - Ogre::TexturePtr dynTexPtr = - Ogre::TextureManager::getSingleton().getByName("rtf"); + Ogre::TexturePtr dynTexPtr = Ogre::TextureManager::getSingleton().getByName("rtf"); //#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) ) // .dynamicCast(); //#else @@ -1137,8 +1046,7 @@ void vpAROgre::getRenderingOutput(vpImage &I, Ogre::RenderTexture *RTarget = dynTexPtr->getBuffer()->getRenderTarget(); mWindow->update(); RTarget->update(); - if (I.getHeight() != mWindow->getHeight() || - I.getWidth() != mWindow->getWidth()) { + if (I.getHeight() != mWindow->getHeight() || I.getWidth() != mWindow->getWidth()) { I.resize(mWindow->getHeight(), mWindow->getWidth()); } Ogre::HardwarePixelBufferSharedPtr mPixelBuffer = dynTexPtr->getBuffer(); diff --git a/modules/ar/src/vpSimulatorException.cpp b/modules/ar/src/vpSimulatorException.cpp index e5300b60ca..221ad76d36 100644 --- a/modules/ar/src/vpSimulatorException.cpp +++ b/modules/ar/src/vpSimulatorException.cpp @@ -43,8 +43,7 @@ #include -vpSimulatorException::vpSimulatorException(const int id, const char *format, - ...) +vpSimulatorException::vpSimulatorException(const int id, const char *format, ...) { this->code = id; va_list args; @@ -53,14 +52,6 @@ vpSimulatorException::vpSimulatorException(const int id, const char *format, va_end(args); } -vpSimulatorException::vpSimulatorException(const int id, - const std::string &msg) - : vpException(id, msg) -{ - ; -} +vpSimulatorException::vpSimulatorException(const int id, const std::string &msg) : vpException(id, msg) { ; } -vpSimulatorException::vpSimulatorException(const int id) : vpException(id) -{ - ; -} +vpSimulatorException::vpSimulatorException(const int id) : vpException(id) { ; } diff --git a/modules/core/include/visp3/core/vpArray2D.h b/modules/core/include/visp3/core/vpArray2D.h index 49ff03a09c..7e3415096e 100644 --- a/modules/core/include/visp3/core/vpArray2D.h +++ b/modules/core/include/visp3/core/vpArray2D.h @@ -88,15 +88,11 @@ template class vpArray2D Basic constructor of a 2D array. Number of columns and rows are set to zero. */ - vpArray2D() - : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL) - { - } + vpArray2D() : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL) {} /*! Copy constructor of a 2D array. */ - vpArray2D(const vpArray2D &A) - : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL) + vpArray2D(const vpArray2D &A) : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL) { resize(A.rowNum, A.colNum, false, false); memcpy(data, A.data, rowNum * colNum * sizeof(Type)); @@ -107,8 +103,7 @@ template class vpArray2D \param r : Array number of rows. \param c : Array number of columns. */ - vpArray2D(unsigned int r, unsigned int c) - : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL) + vpArray2D(unsigned int r, unsigned int c) : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL) { resize(r, c); } @@ -119,8 +114,7 @@ template class vpArray2D \param c : Array number of columns. \param val : Each element of the array is set to \e val. */ - vpArray2D(unsigned int r, unsigned int c, Type val) - : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL) + vpArray2D(unsigned int r, unsigned int c, Type val) : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL) { resize(r, c, false, false); *this = val; @@ -174,8 +168,8 @@ template class vpArray2D \param recopy_ : if true, will perform an explicit recopy of the old data if needed and if flagNullify is set to false. */ - void resize(const unsigned int nrows, const unsigned int ncols, - const bool flagNullify = true, const bool recopy_ = true) + void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify = true, + const bool recopy_ = true) { if ((nrows == rowNum) && (ncols == colNum)) { if (flagNullify && this->data != NULL) { @@ -183,8 +177,7 @@ template class vpArray2D } } else { bool recopy = !flagNullify && recopy_; // priority to flagNullify - const bool recopyNeeded = (ncols != this->colNum && this->colNum > 0 && - ncols > 0 && (!flagNullify || recopy)); + const bool recopyNeeded = (ncols != this->colNum && this->colNum > 0 && ncols > 0 && (!flagNullify || recopy)); Type *copyTmp = NULL; unsigned int rowTmp = 0, colTmp = 0; @@ -203,18 +196,15 @@ template class vpArray2D if ((NULL == this->data) && (0 != this->dsize)) { if (copyTmp != NULL) delete[] copyTmp; - throw(vpException( - vpException::memoryAllocationError, - "Memory allocation error when allocating 2D array data")); + 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")); + throw(vpException(vpException::memoryAllocationError, + "Memory allocation error when allocating 2D array rowPtrs")); } // Update rowPtrs @@ -233,10 +223,8 @@ template class vpArray2D 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; + 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)) { @@ -327,8 +315,7 @@ template class vpArray2D \sa save() */ - static bool load(const std::string &filename, vpArray2D &A, - const bool binary = false, char *header = NULL) + static bool load(const std::string &filename, vpArray2D &A, const bool binary = false, char *header = NULL) { std::fstream file; @@ -366,9 +353,8 @@ template class vpArray2D } while (!headerIsDecoded); if (header != NULL) { -#if defined(__MINGW32__) || \ - !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if defined(__MINGW32__) || \ + !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX sprintf(header, "%s", h.c_str()); #else _snprintf_s(header, h.size() + 1, _TRUNCATE, "%s", h.c_str()); @@ -379,10 +365,8 @@ template class vpArray2D file >> rows; file >> cols; - if (rows >= (std::numeric_limits::max)() || - cols >= (std::numeric_limits::max)()) - throw vpException(vpException::badValue, - "Array exceed the max size."); + if (rows >= (std::numeric_limits::max)() || cols >= (std::numeric_limits::max)()) + throw vpException(vpException::badValue, "Array exceed the max size."); A.resize(rows, cols); @@ -402,9 +386,8 @@ template class vpArray2D h += c; } if (header != NULL) { -#if defined(__MINGW32__) || \ - !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if defined(__MINGW32__) || \ + !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX sprintf(header, "%s", h.c_str()); #else _snprintf_s(header, h.size() + 1, _TRUNCATE, "%s", h.c_str()); @@ -440,8 +423,7 @@ template class vpArray2D \sa saveYAML() */ - static bool loadYAML(const std::string &filename, vpArray2D &A, - char *header = NULL) + static bool loadYAML(const std::string &filename, vpArray2D &A, char *header = NULL) { std::fstream file; @@ -485,8 +467,7 @@ template class vpArray2D // get indentation level which is common to all lines lineStart = (unsigned int)line.find("[") + 1; } - std::stringstream ss( - line.substr(lineStart, line.find("]") - lineStart)); + std::stringstream ss(line.substr(lineStart, line.find("]") - lineStart)); j = 0; while (getline(ss, subs, ',')) A[i][j++] = atof(subs.c_str()); @@ -496,9 +477,8 @@ template class vpArray2D if (header != NULL) { std::string h_ = h.substr(0, h.size() - 1); // Remove last '\n' char -#if defined(__MINGW32__) || \ - !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if defined(__MINGW32__) || \ + !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX sprintf(header, "%s", h_.c_str()); #else _snprintf_s(header, h_.size() + 1, _TRUNCATE, "%s", h_.c_str()); @@ -525,8 +505,8 @@ template class vpArray2D \sa load() */ - static bool save(const std::string &filename, const vpArray2D &A, - const bool binary = false, const char *header = "") + static bool save(const std::string &filename, const vpArray2D &A, const bool binary = false, + const char *header = "") { std::fstream file; @@ -614,8 +594,7 @@ template class vpArray2D \sa loadYAML() */ - static bool saveYAML(const std::string &filename, const vpArray2D &A, - const char *header = "") + static bool saveYAML(const std::string &filename, const vpArray2D &A, const char *header = "") { std::fstream file; @@ -708,12 +687,10 @@ template Type vpArray2D::getMaxValue() const \return m1.hadamard(m2) The Hadamard product : \f$ m1 \circ m2 = (m1 \circ m2)_{i,j} = (m1)_{i,j} (m2)_{i,j} \f$ */ -template -vpArray2D vpArray2D::hadamard(const vpArray2D &m) const +template vpArray2D vpArray2D::hadamard(const vpArray2D &m) const { if (m.getRows() != rowNum || m.getCols() != colNum) { - throw(vpException(vpException::dimensionError, - "Hadamard product: bad dimensions!")); + throw(vpException(vpException::dimensionError, "Hadamard product: bad dimensions!")); } vpArray2D out; diff --git a/modules/core/include/visp3/core/vpBSpline.h b/modules/core/include/visp3/core/vpBSpline.h index bf155ec2ec..61a522efcd 100644 --- a/modules/core/include/visp3/core/vpBSpline.h +++ b/modules/core/include/visp3/core/vpBSpline.h @@ -184,8 +184,7 @@ public /*protected*/: inline void set_controlPoints(const std::list &list) { controlPoints.clear(); - for (std::list::const_iterator it = list.begin(); - it != list.end(); ++it) { + for (std::list::const_iterator it = list.begin(); it != list.end(); ++it) { controlPoints.push_back(*it); } } @@ -198,8 +197,7 @@ public /*protected*/: inline void set_knots(const std::list &list) { knots.clear(); - for (std::list::const_iterator it = list.begin(); - it != list.end(); ++it) { + for (std::list::const_iterator it = list.begin(); it != list.end(); ++it) { knots.push_back(*it); } } @@ -213,37 +211,28 @@ public /*protected*/: inline void set_crossingPoints(const std::list &list) { crossingPoints.clear(); - for (std::list::const_iterator it = list.begin(); - it != list.end(); ++it) { + for (std::list::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 &l_knots); + static unsigned int findSpan(double l_u, unsigned int l_p, std::vector &l_knots); unsigned int findSpan(double u); - static vpBasisFunction *computeBasisFuns(double l_u, unsigned int l_i, - unsigned int l_p, + static vpBasisFunction *computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector &l_knots); vpBasisFunction *computeBasisFuns(double u); - static vpBasisFunction **computeDersBasisFuns(double l_u, unsigned int l_i, - unsigned int l_p, - unsigned int l_der, + static vpBasisFunction **computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector &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 &l_knots, - std::vector &l_controlPoints); + static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector &l_knots, + std::vector &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 &l_knots, - std::vector &l_controlPoints); + static vpImagePoint *computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, + std::vector &l_knots, std::vector &l_controlPoints); vpImagePoint *computeCurveDers(double u, unsigned int der); }; diff --git a/modules/core/include/visp3/core/vpCameraParameters.h b/modules/core/include/visp3/core/vpCameraParameters.h index 444f192b90..5523eeaf0e 100644 --- a/modules/core/include/visp3/core/vpCameraParameters.h +++ b/modules/core/include/visp3/core/vpCameraParameters.h @@ -189,27 +189,34 @@ 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; + 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); + // model + cam.initPersProjWithoutDistortion(px,py,u0,v0); // It is also possible to print the current camera parameters std::cout << cam << std::endl; - \endcode +\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; + 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 + model cam.initPersProjWithDistortion(px,py,u0,v0,kud,kdu); +\endcode The code below shows how to know the currently used projection model: \code @@ -231,17 +238,16 @@ class VISP_EXPORT vpCameraParameters typedef enum { perspectiveProjWithoutDistortion, //!< Perspective projection without //!< distortion model - perspectiveProjWithDistortion //!< Perspective projection with 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(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(); @@ -249,13 +255,10 @@ class VISP_EXPORT 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); + 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. @@ -327,17 +330,13 @@ class VISP_EXPORT vpCameraParameters inline double get_kud() const { return kud; } inline double get_kdu() const { return kdu; } - inline vpCameraParametersProjType get_projModel() const - { - return projModel; - } + 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); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCameraParameters &cam); private: static const double DEFAULT_U0_PARAMETER; @@ -353,13 +352,12 @@ class VISP_EXPORT vpCameraParameters 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 - fovNormals; //!< Normals of the planes describing the fov + 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 fovNormals; //!< Normals of the planes describing the fov double inv_px, inv_py; diff --git a/modules/core/include/visp3/core/vpCircle.h b/modules/core/include/visp3/core/vpCircle.h index fc018b510d..87c3952888 100644 --- a/modules/core/include/visp3/core/vpCircle.h +++ b/modules/core/include/visp3/core/vpCircle.h @@ -61,14 +61,13 @@ class VISP_EXPORT vpCircle : public vpForwardProjection void init(); vpCircle(); explicit 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); + 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); + 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]; } @@ -85,13 +84,10 @@ class VISP_EXPORT vpCircle : public vpForwardProjection void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP); void changeFrame(const vpHomogeneousMatrix &cMo); - void display(const vpImage &I, const vpCameraParameters &cam, - const vpColor &color = vpColor::green, - const unsigned int thickness = 1); - void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &color = vpColor::green, + void display(const vpImage &I, const vpCameraParameters &cam, const vpColor &color = vpColor::green, const unsigned int thickness = 1); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &color = vpColor::green, const unsigned int thickness = 1); vpCircle *duplicate() const; //################### @@ -99,10 +95,8 @@ class VISP_EXPORT vpCircle : public vpForwardProjection //################### public: - static void computeIntersectionPoint(const vpCircle &circle, - const vpCameraParameters &cam, - const double &rho, const double &theta, - double &i, double &j); + 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 index 039a5f9e97..36551d822f 100644 --- a/modules/core/include/visp3/core/vpClient.h +++ b/modules/core/include/visp3/core/vpClient.h @@ -77,13 +77,17 @@ int main(int argc, char **argv) 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; + // Sending the new value to the first client + if(client.send(&val) != sizeof(int)) + std::cout << "Error while sending" << std::endl; + else + std::cout << Sending : " << val << std::endl; + + // Receiving a value from the first client + if(client.receive(&val) != sizeof(int)) + std::cout << "Error while receiving" << std::endl; + else + std::cout << "Received : " << val << std::endl; } return 0; @@ -113,7 +117,11 @@ int main(int argc, char **argv) vpImage 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); + // Linux) + vpV4l2Grabber g; + g.setScale(1); + g.setInput(0); + g.open(I); // Create an image viewer #if defined(VISP_HAVE_X11) @@ -172,8 +180,7 @@ class VISP_EXPORT vpClient : public vpNetwork vpClient(); virtual ~vpClient(); - bool connectToHostname(const std::string &hostname, - const unsigned int &port_serv); + 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); @@ -191,10 +198,7 @@ class VISP_EXPORT vpClient : public vpNetwork \return Number of servers. */ - unsigned int getNumberOfServers() - { - return (unsigned int)receptor_list.size(); - } + unsigned int getNumberOfServers() { return (unsigned int)receptor_list.size(); } void print(); diff --git a/modules/core/include/visp3/core/vpColVector.h b/modules/core/include/visp3/core/vpColVector.h index fd39cbe688..8243cd0baa 100644 --- a/modules/core/include/visp3/core/vpColVector.h +++ b/modules/core/include/visp3/core/vpColVector.h @@ -124,9 +124,7 @@ class VISP_EXPORT vpColVector : public vpArray2D rowNum = colNum = dsize = 0; } - std::ostream &cppPrint(std::ostream &os, - const std::string &matrixName = "A", - bool octet = false) const; + std::ostream &cppPrint(std::ostream &os, const std::string &matrixName = "A", bool octet = false) const; std::ostream &csvPrint(std::ostream &os) const; /*! @@ -185,10 +183,7 @@ class VISP_EXPORT vpColVector : public vpArray2D //! 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); - } + 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); @@ -221,8 +216,7 @@ class VISP_EXPORT vpColVector : public vpArray2D vpColVector &operator<<(const vpColVector &v); vpColVector &operator<<(double *); - int print(std::ostream &s, unsigned int length, - char const *intro = 0) const; + int print(std::ostream &s, unsigned int length, char const *intro = 0) const; /*! Convert a column vector containing angles in radians into degrees. @@ -235,8 +229,7 @@ class VISP_EXPORT vpColVector : public vpArray2D (*this) *= r2d; } - void reshape(vpMatrix &M, const unsigned int &nrows, - const unsigned int &ncols); + 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. @@ -245,10 +238,7 @@ class VISP_EXPORT vpColVector : public vpArray2D \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::resize(i, 1, flagNullify); - } + void resize(const unsigned int i, const bool flagNullify = true) { vpArray2D::resize(i, 1, flagNullify); } /*! Resize the column vector to a \e nrows-dimension vector. This function can only be used with \e ncols = 1. @@ -259,8 +249,7 @@ class VISP_EXPORT vpColVector : public vpArray2D \exception vpException::fatalError When \e ncols is not equal to 1. */ - void resize(const unsigned int nrows, const unsigned int ncols, - const bool flagNullify) + void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify) { if (ncols != 1) throw(vpException(vpException::fatalError, @@ -288,10 +277,7 @@ class VISP_EXPORT vpColVector : public vpArray2D equal to 3. */ - inline static vpColVector cross(const vpColVector &a, const vpColVector &b) - { - return crossProd(a, b); - } + 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); @@ -304,11 +290,9 @@ class VISP_EXPORT vpColVector : public vpArray2D 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 void stack(const vpColVector &A, const vpColVector &B, vpColVector &C); - static double stdev(const vpColVector &v, - const bool useBesselCorrection = false); + static double stdev(const vpColVector &v, const bool useBesselCorrection = false); #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) /*! @@ -323,8 +307,7 @@ class VISP_EXPORT vpColVector : public vpArray2D /*! \deprecated You should rather use extract(). */ - vp_deprecated vpColVector rows(unsigned int first_row, - unsigned int last_row) const + vp_deprecated vpColVector rows(unsigned int first_row, unsigned int last_row) const { return vpColVector(*this, first_row - 1, last_row - first_row + 1); } @@ -340,23 +323,17 @@ class VISP_EXPORT vpColVector : public vpArray2D \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); - } + 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) + vp_deprecated static void stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C) { stack(A, B, C); } - vp_deprecated void insert(const vpColVector &v, const unsigned int r, - const unsigned int c = 0); + vp_deprecated void insert(const vpColVector &v, const unsigned int r, const unsigned int c = 0); //@} #endif }; diff --git a/modules/core/include/visp3/core/vpColor.h b/modules/core/include/visp3/core/vpColor.h index f7ce8f7fc5..6666f7c647 100644 --- a/modules/core/include/visp3/core/vpColor.h +++ b/modules/core/include/visp3/core/vpColor.h @@ -122,22 +122,22 @@ 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_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 @@ -146,18 +146,18 @@ class VISP_EXPORT vpColor : public vpRGBa 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_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 @@ -246,109 +246,87 @@ class VISP_EXPORT vpColor : public vpRGBa \param i : color indice */ - static inline vpColor getColor(const unsigned int &i) - { - return vpColor::allColors[i % vpColor::nbColors]; - }; + 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); +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); +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); +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); +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); +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); +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); +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); +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); +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); +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); +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); +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); +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); +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); +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); +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); +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); +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 +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 diff --git a/modules/core/include/visp3/core/vpConvert.h b/modules/core/include/visp3/core/vpConvert.h index 78b2815981..20306d0a46 100644 --- a/modules/core/include/visp3/core/vpConvert.h +++ b/modules/core/include/visp3/core/vpConvert.h @@ -68,42 +68,28 @@ class VISP_EXPORT vpConvert 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 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 &from, - std::vector &to); - static void convertFromOpenCV(const std::vector &from, - std::vector &to); - static void convertFromOpenCV(const std::vector &from, - std::vector &to); - static void convertFromOpenCV(const std::vector &from, - std::vector &to, + static void convertFromOpenCV(const std::vector &from, std::vector &to); + static void convertFromOpenCV(const std::vector &from, std::vector &to); + static void convertFromOpenCV(const std::vector &from, std::vector &to); + static void convertFromOpenCV(const std::vector &from, std::vector &to, const bool cameraFrame = false); - static void convertFromOpenCV(const std::vector &from, - std::vector &to, + static void convertFromOpenCV(const std::vector &from, std::vector &to, const bool cameraFrame = false); - static void convertFromOpenCV(const std::vector &from, - std::vector &to); + static void convertFromOpenCV(const std::vector &from, std::vector &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 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 &from, - std::vector &to); - static void convertToOpenCV(const std::vector &from, - std::vector &to); - static void convertToOpenCV(const std::vector &from, - std::vector &to, + static void convertToOpenCV(const std::vector &from, std::vector &to); + static void convertToOpenCV(const std::vector &from, std::vector &to); + static void convertToOpenCV(const std::vector &from, std::vector &to, const bool cameraFrame = false); - static void convertToOpenCV(const std::vector &from, - std::vector &to, + static void convertToOpenCV(const std::vector &from, std::vector &to, const bool cameraFrame = false); private: diff --git a/modules/core/include/visp3/core/vpCylinder.h b/modules/core/include/visp3/core/vpCylinder.h index 30e7f81b02..ef700bd13c 100644 --- a/modules/core/include/visp3/core/vpCylinder.h +++ b/modules/core/include/visp3/core/vpCylinder.h @@ -103,8 +103,8 @@ class VISP_EXPORT vpCylinder : public vpForwardProjection vpCylinder(); explicit 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); + 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); @@ -112,13 +112,10 @@ class VISP_EXPORT vpCylinder : public vpForwardProjection double computeZ(const double x, const double y) const; - void display(const vpImage &I, const vpCameraParameters &cam, - const vpColor &color = vpColor::green, - const unsigned int thickness = 1); - void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &color = vpColor::green, + void display(const vpImage &I, const vpCameraParameters &cam, const vpColor &color = vpColor::green, const unsigned int thickness = 1); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &color = vpColor::green, const unsigned int thickness = 1); vpCylinder *duplicate() const; @@ -183,9 +180,8 @@ class VISP_EXPORT vpCylinder : public vpForwardProjection 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); + 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 index 7cf85d5245..51aac476ee 100644 --- a/modules/core/include/visp3/core/vpDebug.h +++ b/modules/core/include/visp3/core/vpDebug.h @@ -157,10 +157,8 @@ class vpTraceOutput \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) + 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) { } @@ -226,8 +224,7 @@ class vpTraceOutput if (header != NULL) std::cerr << header; // then writes the recorded namefile, function and line - std::cerr << "!!\t" << currentFile << ": " << currentFunc << "(#" - << currentLine << ") : "; + std::cerr << "!!\t" << currentFile << ": " << currentFunc << "(#" << currentLine << ") : "; // and finally writes the message passed to () operator. vfprintf(stderr, format, args); fprintf(stderr, "\n"); @@ -238,8 +235,7 @@ class vpTraceOutput if (header != NULL) std::cout << header; // then writes the recorded namefile, function and line - std::cout << currentFile << ": " << currentFunc << "(#" << currentLine - << ") : "; + std::cout << currentFile << ": " << currentFunc << "(#" << currentLine << ") : "; // and finally writes the message passed to () operator. vprintf(format, args); printf("\n"); @@ -276,8 +272,7 @@ int main() \sa vpOUT_FCT */ -#define vpIN_FCT \ - (vpTraceOutput(__FILE__, __LINE__, __FUNCTION__, false, "begin ")) +#define vpIN_FCT (vpTraceOutput(__FILE__, __LINE__, __FUNCTION__, false, "begin ")) /*! \ingroup group_core_debug @@ -297,8 +292,7 @@ int main() \sa vpIN_FCT */ -#define vpOUT_FCT \ - (vpTraceOutput(__FILE__, __LINE__, __FUNCTION__, false, "end ")) +#define vpOUT_FCT (vpTraceOutput(__FILE__, __LINE__, __FUNCTION__, false, "end ")) #else // #ifdef VP_TRACE @@ -341,9 +335,7 @@ int main() \sa vpTRACE(), vpCERROR(), vpCDEBUG() */ -#define vpCTRACE \ - std::cout << "(L0) " << __FILE__ << ": " << __FUNCTION__ << "(#" \ - << __LINE__ << ") : " +#define vpCTRACE std::cout << "(L0) " << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") : " /*! \ingroup group_core_debug @@ -370,10 +362,9 @@ int main() \sa vpCTRACE(), vpCDEBUG() */ -#define vpCERROR \ - std::cerr << "(L0) " \ - << "!!\t" << __FILE__ << ": " << __FUNCTION__ << "(#" \ - << __LINE__ << ") : " +#define vpCERROR \ + std::cerr << "(L0) " \ + << "!!\t" << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") : " /*! \ingroup group_core_debug @@ -426,11 +417,11 @@ int main() #else // #ifdef VP_TRACE -#define vpCTRACE \ - if (false) \ +#define vpCTRACE \ + if (false) \ std::cout // Warning C4127 -#define vpCERROR \ - if (false) \ +#define vpCERROR \ + if (false) \ std::cerr // Warning C4127 inline void vpERROR_TRACE(const char * /* a */, ...) {} @@ -517,12 +508,11 @@ int main() \sa vpCTRACE(), vpCERROR() */ -#define vpCDEBUG(level) \ - if (VP_DEBUG_MODE < level) \ - ; \ - else \ - std::cout << "(L" << level << ") " << __FILE__ << ": " << __FUNCTION__ \ - << "(#" << __LINE__ << ") : " +#define vpCDEBUG(level) \ + if (VP_DEBUG_MODE < level) \ + ; \ + else \ + std::cout << "(L" << level << ") " << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") : " /*! \ingroup group_core_debug @@ -554,8 +544,8 @@ 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) \ +#define vpCDEBUG(level) \ + if (false) \ std::cout // Warning C4127 #define vpDEBUG_ENABLE(level) (false) // Warning C4127 diff --git a/modules/core/include/visp3/core/vpDisplay.h b/modules/core/include/visp3/core/vpDisplay.h index b81ad0db38..c01b46f2d6 100644 --- a/modules/core/include/visp3/core/vpDisplay.h +++ b/modules/core/include/visp3/core/vpDisplay.h @@ -133,7 +133,7 @@ int main() topLeftCorner.set_i(50); topLeftCorner.set_j(10); vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, -true); + true); // Flush the foreground and background display vpDisplay::flush(I); @@ -145,8 +145,9 @@ true); 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); + std::cout << "keyboard event: key: " << "\"" << key + << "\"" << std::endl; + vpTime::wait(40); } // Get a blocking keyboard event @@ -173,27 +174,27 @@ class VISP_EXPORT vpDisplay //! Values that could be applied to a display to down scale the size of the //! display. typedef enum { - SCALE_AUTO, /*!< Auto down scaling factor computed fom the screen - resolution. */ - SCALE_1, /*!< Display and image have the same size. */ - SCALE_2, /*!< Display width and height is down scaled by 2 wrt the image - size. */ - SCALE_3, /*!< Display width and height is down scaled by 3 wrt the image - size. */ - SCALE_4, /*!< Display width and height is down scaled by 4 wrt the image - size. */ - SCALE_5, /*!< Display width and height is down scaled by 5 wrt the image - size. */ - SCALE_6, /*!< Display width and height is down scaled by 6 wrt the image - size. */ - SCALE_7, /*!< Display width and height is down scaled by 7 wrt the image - size. */ - SCALE_8, /*!< Display width and height is down scaled by 8 wrt the image - size. */ - SCALE_9, /*!< Display width and height is down scaled by 9 wrt the image - size. */ - SCALE_10, /*!< Display width and height is down scaled by 10 wrt the image - size. */ + SCALE_AUTO, /*!< Auto down scaling factor computed fom the screen + resolution. */ + SCALE_1, /*!< Display and image have the same size. */ + SCALE_2, /*!< Display width and height is down scaled by 2 wrt the image + size. */ + SCALE_3, /*!< Display width and height is down scaled by 3 wrt the image + size. */ + SCALE_4, /*!< Display width and height is down scaled by 4 wrt the image + size. */ + SCALE_5, /*!< Display width and height is down scaled by 5 wrt the image + size. */ + SCALE_6, /*!< Display width and height is down scaled by 6 wrt the image + size. */ + SCALE_7, /*!< Display width and height is down scaled by 7 wrt the image + size. */ + SCALE_8, /*!< Display width and height is down scaled by 8 wrt the image + size. */ + SCALE_9, /*!< Display width and height is down scaled by 9 wrt the image + size. */ + SCALE_10, /*!< Display width and height is down scaled by 10 wrt the image + size. */ SCALE_DEFAULT /*!< Display and image have the same size. Similar to vpDisplay::SCALE_1. */ } vpScaleType; @@ -211,8 +212,7 @@ class VISP_EXPORT vpDisplay unsigned int m_scale; vpScaleType m_scaleType; - void setScale(vpScaleType scaleType, unsigned int width, - unsigned int height); + void setScale(vpScaleType scaleType, unsigned int width, unsigned int height); public: vpDisplay(); @@ -281,10 +281,8 @@ class VISP_EXPORT vpDisplay \param h : Arrow height. \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; + 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. @@ -296,8 +294,7 @@ class VISP_EXPORT vpDisplay \sa setFont() */ - virtual void displayCharString(const vpImagePoint &ip, const char *text, - const vpColor &color = vpColor::green) = 0; + virtual void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color = vpColor::green) = 0; /*! Display a circle. \param center : Circle center position. @@ -307,8 +304,7 @@ class VISP_EXPORT vpDisplay \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, + 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. @@ -317,8 +313,7 @@ class VISP_EXPORT vpDisplay \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, + 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. @@ -327,8 +322,7 @@ class VISP_EXPORT vpDisplay \param color : Line color. \param thickness : Dashed line thickness. */ - virtual void displayDotLine(const vpImagePoint &ip1, - const vpImagePoint &ip2, const vpColor &color, + 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. @@ -337,8 +331,7 @@ class VISP_EXPORT vpDisplay \param color : Line color. \param thickness : Line thickness. */ - virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, - const vpColor &color, + virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness = 1) = 0; /*! @@ -367,13 +360,9 @@ class VISP_EXPORT vpDisplay */ virtual void displayImage(const vpImage &I) = 0; - virtual void displayImageROI(const vpImage &I, - const vpImagePoint &iP, - const unsigned int width, + virtual void displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height) = 0; - virtual void displayImageROI(const vpImage &I, - const vpImagePoint &iP, - const unsigned int width, + virtual void displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height) = 0; /*! @@ -382,8 +371,7 @@ class VISP_EXPORT vpDisplay \param color : Point color. \param thickness : Point thickness. */ - virtual void displayPoint(const vpImagePoint &ip, const vpColor &color, - unsigned int thickness = 1) = 0; + virtual void displayPoint(const vpImagePoint &ip, const vpColor &color, unsigned int thickness = 1) = 0; /*! Display a rectangle with \e topLeft as the top-left corner and \e @@ -399,10 +387,8 @@ class VISP_EXPORT vpDisplay 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; + 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. @@ -416,10 +402,8 @@ class VISP_EXPORT vpDisplay 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; + 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. @@ -433,8 +417,7 @@ class VISP_EXPORT vpDisplay false. */ - virtual void displayRectangle(const vpRect &rectangle, const vpColor &color, - bool fill = false, + virtual void displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness = 1) = 0; /*! @@ -447,9 +430,7 @@ class VISP_EXPORT vpDisplay 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; + virtual void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height) = 0; /* Simple interface with the mouse event */ @@ -504,9 +485,7 @@ class VISP_EXPORT vpDisplay 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; + 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 @@ -529,9 +508,7 @@ class VISP_EXPORT vpDisplay \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool) */ - virtual bool getClickUp(vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, - bool blocking = true) = 0; + virtual bool getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true) = 0; /*! Get a keyboard event. @@ -615,8 +592,7 @@ class VISP_EXPORT vpDisplay \param y : Vertical position of the upper/left window corner. \param title : Window title. */ - virtual void init(vpImage &I, int x = -1, int y = -1, - const std::string &title = "") = 0; + virtual void init(vpImage &I, int x = -1, int y = -1, const std::string &title = "") = 0; /*! Initialize the display (size, position and title) of a color image in RGBa format. @@ -626,8 +602,7 @@ class VISP_EXPORT vpDisplay \param y : Vertical position of the upper/left window corner. \param title : Window title. */ - virtual void init(vpImage &I, int x = -1, int y = -1, - const std::string &title = "") = 0; + virtual void init(vpImage &I, int x = -1, int y = -1, const std::string &title = "") = 0; /*! Initialize the display size, position and title. @@ -689,8 +664,7 @@ int main() } \endcode */ - virtual void init(unsigned int width, unsigned int height, int x = -1, - int y = -1, const std::string &title = "") = 0; + virtual void init(unsigned int width, unsigned int height, int x = -1, int y = -1, const std::string &title = "") = 0; /*! Set the font used to display a text in overlay. The display is @@ -728,158 +702,98 @@ int main() //@{ static void close(vpImage &I); static void display(const vpImage &I); - static void displayArrow(const vpImage &I, - const vpImagePoint &ip1, const vpImagePoint &ip2, - const vpColor &color = vpColor::white, - unsigned int w = 4, unsigned int h = 2, + static void displayArrow(const vpImage &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 &I, int i1, int j1, - int i2, int j2, - const vpColor &color = vpColor::white, - unsigned int w = 4, unsigned int h = 2, + static void displayArrow(const vpImage &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 &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, double size, - const vpColor &color, unsigned int thickness); - static void displayCharString(const vpImage &I, - const vpImagePoint &ip, const char *string, + static void displayCamera(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness); + static void displayCharString(const vpImage &I, const vpImagePoint &ip, const char *string, const vpColor &color); - static void displayCharString(const vpImage &I, int i, int j, - const char *string, const vpColor &color); - static void displayCircle(const vpImage &I, - const vpImagePoint ¢er, unsigned int radius, - const vpColor &color, bool fill = false, - unsigned int thickness = 1); - static void displayCircle(const vpImage &I, int i, int j, - unsigned int radius, const vpColor &color, + static void displayCharString(const vpImage &I, int i, int j, const char *string, + const vpColor &color); + static void displayCircle(const vpImage &I, const vpImagePoint ¢er, unsigned int radius, + const vpColor &color, bool fill = false, unsigned int thickness = 1); + static void displayCircle(const vpImage &I, int i, int j, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness = 1); - static void displayCross(const vpImage &I, - const vpImagePoint &ip, unsigned int size, + static void displayCross(const vpImage &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness = 1); - static void displayCross(const vpImage &I, int i, int j, - unsigned int size, const vpColor &color, + static void displayCross(const vpImage &I, int i, int j, unsigned int size, const vpColor &color, unsigned int thickness = 1); - static void displayDotLine(const vpImage &I, - const vpImagePoint &ip1, const vpImagePoint &ip2, - const vpColor &color, - unsigned int thickness = 1); - static void displayDotLine(const vpImage &I, int i1, int j1, - int i2, int j2, const vpColor &color, - unsigned int thickness = 1); - static void displayDotLine(const vpImage &I, - const std::vector &ips, - const bool closeTheShape, const vpColor &color, - unsigned int thickness = 1); - static void displayEllipse(const vpImage &I, - const vpImagePoint ¢er, const double &coef1, - const double &coef2, const double &coef3, - bool use_centered_moments, const vpColor &color, + static void displayDotLine(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, + const vpColor &color, unsigned int thickness = 1); + static void displayDotLine(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness = 1); - static void displayEllipse(const vpImage &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, + static void displayDotLine(const vpImage &I, const std::vector &ips, + const bool closeTheShape, const vpColor &color, unsigned int thickness = 1); + static void displayEllipse(const vpImage &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 displayFrame(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, double size, - const vpColor &color = vpColor::none, - unsigned int thickness = 1, - const vpImagePoint &offset = vpImagePoint(0, 0)); - static void displayLine(const vpImage &I, - const vpImagePoint &ip1, const vpImagePoint &ip2, + static void displayEllipse(const vpImage &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 &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &cam, double size, const vpColor &color = vpColor::none, + unsigned int thickness = 1, const vpImagePoint &offset = vpImagePoint(0, 0)); + static void displayLine(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness = 1); - static void displayLine(const vpImage &I, int i1, int j1, - int i2, int j2, const vpColor &color, - unsigned int thickness = 1); - static void displayLine(const vpImage &I, - const std::vector &ips, - const bool closeTheShape, const vpColor &color, + static void displayLine(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness = 1); - static void displayPoint(const vpImage &I, - const vpImagePoint &ip, const vpColor &color, + static void displayLine(const vpImage &I, const std::vector &ips, + const bool closeTheShape, const vpColor &color, unsigned int thickness = 1); + static void displayPoint(const vpImage &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness = 1); - static void displayPoint(const vpImage &I, int i, int j, - const vpColor &color, unsigned int thickness = 1); - static void displayPolygon(const vpImage &I, - const std::vector &vip, - const vpColor &color, - unsigned int thickness = 1); - static void displayRectangle(const vpImage &I, - const vpImagePoint &topLeft, - unsigned int width, unsigned int height, - const vpColor &color, bool fill = false, + static void displayPoint(const vpImage &I, int i, int j, const vpColor &color, + unsigned int thickness = 1); + static void displayPolygon(const vpImage &I, const std::vector &vip, + const vpColor &color, unsigned int thickness = 1); + static void displayRectangle(const vpImage &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 &I, - const vpImagePoint &topLeft, - const vpImagePoint &bottomRight, - const vpColor &color, bool fill = false, + static void displayRectangle(const vpImage &I, const vpImagePoint &topLeft, + const vpImagePoint &bottomRight, const vpColor &color, bool fill = false, unsigned int thickness = 1); - static void displayRectangle(const vpImage &I, - const vpRect &rectangle, const vpColor &color, + static void displayRectangle(const vpImage &I, const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness = 1); - static void displayRectangle(const vpImage &I, - const vpImagePoint ¢er, float angle, - unsigned int width, unsigned int height, - const vpColor &color, - unsigned int thickness = 1); - static void displayRectangle(const vpImage &I, int i, int j, - unsigned int width, unsigned int height, - const vpColor &color, bool fill = false, + static void displayRectangle(const vpImage &I, const vpImagePoint ¢er, float angle, + unsigned int width, unsigned int height, const vpColor &color, unsigned int thickness = 1); - static void displayRectangle(const vpImage &I, - unsigned int i, unsigned int j, float angle, - unsigned int width, unsigned int height, - const vpColor &color, + static void displayRectangle(const vpImage &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 &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 &I, const vpRect &roi); - static void displayText(const vpImage &I, - const vpImagePoint &ip, const std::string &s, + static void displayText(const vpImage &I, const vpImagePoint &ip, const std::string &s, const vpColor &color); - static void displayText(const vpImage &I, int i, int j, - const std::string &s, const vpColor &color); + static void displayText(const vpImage &I, int i, int j, const std::string &s, const vpColor &color); static void flush(const vpImage &I); static void flushROI(const vpImage &I, const vpRect &roi); static bool getClick(const vpImage &I, bool blocking = true); - static bool getClick(const vpImage &I, vpImagePoint &ip, - bool blocking = true); - static bool getClick(const vpImage &I, vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, - bool blocking = true); - static bool getClick(const vpImage &I, - vpMouseButton::vpMouseButtonType &button, + static bool getClick(const vpImage &I, vpImagePoint &ip, bool blocking = true); + static bool getClick(const vpImage &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true); - static bool getClickUp(const vpImage &I, vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, + static bool getClick(const vpImage &I, vpMouseButton::vpMouseButtonType &button, bool blocking = true); + static bool getClickUp(const vpImage &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true); - static bool getClickUp(const vpImage &I, - vpMouseButton::vpMouseButtonType &button, + static bool getClickUp(const vpImage &I, vpMouseButton::vpMouseButtonType &button, bool blocking = true); static unsigned int getDownScalingFactor(const vpImage &I); static void getImage(const vpImage &Is, vpImage &Id); - static bool getKeyboardEvent(const vpImage &I, - bool blocking = true); - static bool getKeyboardEvent(const vpImage &I, - std::string &key, bool blocking = true); - static bool getKeyboardEvent(const vpImage &I, char *key, - bool blocking = true); - static bool getPointerMotionEvent(const vpImage &I, - vpImagePoint &ip); - static bool getPointerPosition(const vpImage &I, - vpImagePoint &ip); - - static void setBackground(const vpImage &I, - const vpColor &color); - static void setFont(const vpImage &I, - const std::string &font); - static void setTitle(const vpImage &I, - const std::string &windowtitle); - static void setWindowPosition(const vpImage &I, int winx, - int winy); + static bool getKeyboardEvent(const vpImage &I, bool blocking = true); + static bool getKeyboardEvent(const vpImage &I, std::string &key, bool blocking = true); + static bool getKeyboardEvent(const vpImage &I, char *key, bool blocking = true); + static bool getPointerMotionEvent(const vpImage &I, vpImagePoint &ip); + static bool getPointerPosition(const vpImage &I, vpImagePoint &ip); + + static void setBackground(const vpImage &I, const vpColor &color); + static void setFont(const vpImage &I, const std::string &font); + static void setTitle(const vpImage &I, const std::string &windowtitle); + static void setWindowPosition(const vpImage &I, int winx, int winy); //@} /*! @@ -888,150 +802,91 @@ int main() //@{ static void close(vpImage &I); static void display(const vpImage &I); - static void displayArrow(const vpImage &I, const vpImagePoint &ip1, - const vpImagePoint &ip2, - const vpColor &color = vpColor::white, - unsigned int w = 4, unsigned int h = 2, + static void displayArrow(const vpImage &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 &I, int i1, int j1, int i2, - int j2, const vpColor &color = vpColor::white, - unsigned int w = 4, unsigned int h = 2, + static void displayArrow(const vpImage &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 &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, double size, - const vpColor &color, unsigned int thickness); - static void displayCharString(const vpImage &I, - const vpImagePoint &ip, const char *string, + static void displayCamera(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + double size, const vpColor &color, unsigned int thickness); + static void displayCharString(const vpImage &I, const vpImagePoint &ip, const char *string, const vpColor &color); - static void displayCharString(const vpImage &I, int i, int j, - const char *string, const vpColor &color); - static void displayCircle(const vpImage &I, - const vpImagePoint ¢er, unsigned int radius, - const vpColor &color, bool fill = false, - unsigned int thickness = 1); - static void displayCircle(const vpImage &I, int i, int j, - unsigned int radius, const vpColor &color, + static void displayCharString(const vpImage &I, int i, int j, const char *string, const vpColor &color); + static void displayCircle(const vpImage &I, const vpImagePoint ¢er, unsigned int radius, + const vpColor &color, bool fill = false, unsigned int thickness = 1); + static void displayCircle(const vpImage &I, int i, int j, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness = 1); - static void displayCross(const vpImage &I, const vpImagePoint &ip, - unsigned int size, const vpColor &color, + static void displayCross(const vpImage &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness = 1); - static void displayCross(const vpImage &I, int i, int j, - unsigned int size, const vpColor &color, + static void displayCross(const vpImage &I, int i, int j, unsigned int size, const vpColor &color, unsigned int thickness = 1); - static void displayDotLine(const vpImage &I, - const vpImagePoint &ip1, const vpImagePoint &ip2, - const vpColor &color, + static void displayDotLine(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, + const vpColor &color, unsigned int thickness = 1); + static void displayDotLine(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness = 1); - static void displayDotLine(const vpImage &I, int i1, int j1, int i2, - int j2, const vpColor &color, + static void displayDotLine(const vpImage &I, const std::vector &ips, const bool closeTheShape, + const vpColor &color, unsigned int thickness = 1); + static void displayEllipse(const vpImage &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 displayDotLine(const vpImage &I, - const std::vector &ips, - const bool closeTheShape, const vpColor &color, - unsigned int thickness = 1); - static void displayEllipse(const vpImage &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 &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 &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, double size, - const vpColor &color = vpColor::none, - unsigned int thickness = 1, + static void displayEllipse(const vpImage &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 &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + double size, const vpColor &color = vpColor::none, unsigned int thickness = 1, const vpImagePoint &offset = vpImagePoint(0, 0)); - static void displayLine(const vpImage &I, const vpImagePoint &ip1, - const vpImagePoint &ip2, const vpColor &color, - unsigned int thickness = 1); - static void displayLine(const vpImage &I, int i1, int j1, int i2, - int j2, const vpColor &color, - unsigned int thickness = 1); - static void displayLine(const vpImage &I, - const std::vector &ips, - const bool closeTheShape, const vpColor &color, + static void displayLine(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, + const vpColor &color, unsigned int thickness = 1); + static void displayLine(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness = 1); - static void displayPoint(const vpImage &I, const vpImagePoint &ip, - const vpColor &color, unsigned int thickness = 1); - static void displayPoint(const vpImage &I, int i, int j, - const vpColor &color, unsigned int thickness = 1); - static void displayPolygon(const vpImage &I, - const std::vector &vip, - const vpColor &color, + static void displayLine(const vpImage &I, const std::vector &ips, const bool closeTheShape, + const vpColor &color, unsigned int thickness = 1); + static void displayPoint(const vpImage &I, const vpImagePoint &ip, const vpColor &color, + unsigned int thickness = 1); + static void displayPoint(const vpImage &I, int i, int j, const vpColor &color, unsigned int thickness = 1); + static void displayPolygon(const vpImage &I, const std::vector &vip, const vpColor &color, unsigned int thickness = 1); - static void displayRectangle(const vpImage &I, - const vpImagePoint &topLeft, - unsigned int width, unsigned int height, - const vpColor &color, bool fill = false, + static void displayRectangle(const vpImage &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 &I, - const vpImagePoint &topLeft, - const vpImagePoint &bottomRight, - const vpColor &color, bool fill = false, - unsigned int thickness = 1); - static void displayRectangle(const vpImage &I, - const vpRect &rectangle, const vpColor &color, + static void displayRectangle(const vpImage &I, const vpImagePoint &topLeft, const vpImagePoint &bottomRight, + const vpColor &color, bool fill = false, unsigned int thickness = 1); + static void displayRectangle(const vpImage &I, const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness = 1); - static void displayRectangle(const vpImage &I, - const vpImagePoint ¢er, float angle, - unsigned int width, unsigned int height, - const vpColor &color, - unsigned int thickness = 1); - static void displayRectangle(const vpImage &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 &I, unsigned int i, - unsigned int j, float angle, - unsigned int width, unsigned int height, - const vpColor &color, + static void displayRectangle(const vpImage &I, const vpImagePoint ¢er, float angle, unsigned int width, + unsigned int height, const vpColor &color, unsigned int thickness = 1); + static void displayRectangle(const vpImage &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 &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 &I, const vpRect &roi); - static void displayText(const vpImage &I, const vpImagePoint &ip, - const std::string &s, const vpColor &color); - static void displayText(const vpImage &I, int i, int j, - const std::string &s, const vpColor &color); + static void displayText(const vpImage &I, const vpImagePoint &ip, const std::string &s, const vpColor &color); + static void displayText(const vpImage &I, int i, int j, const std::string &s, const vpColor &color); static void flush(const vpImage &I); static void flushROI(const vpImage &I, const vpRect &roi); static bool getClick(const vpImage &I, bool blocking = true); - static bool getClick(const vpImage &I, vpImagePoint &ip, - bool blocking = true); - static bool getClick(const vpImage &I, vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, + static bool getClick(const vpImage &I, vpImagePoint &ip, bool blocking = true); + static bool getClick(const vpImage &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true); - static bool getClick(const vpImage &I, - vpMouseButton::vpMouseButtonType &button, - bool blocking = true); - static bool getClickUp(const vpImage &I, vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, - bool blocking = true); - static bool getClickUp(const vpImage &I, - vpMouseButton::vpMouseButtonType &button, + static bool getClick(const vpImage &I, vpMouseButton::vpMouseButtonType &button, bool blocking = true); + static bool getClickUp(const vpImage &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true); + static bool getClickUp(const vpImage &I, vpMouseButton::vpMouseButtonType &button, bool blocking = true); static unsigned int getDownScalingFactor(const vpImage &I); static void getImage(const vpImage &Is, vpImage &Id); - static bool getKeyboardEvent(const vpImage &I, - bool blocking = true); - static bool getKeyboardEvent(const vpImage &I, std::string &key, - bool blocking = true); - static bool getKeyboardEvent(const vpImage &I, char *key, - bool blocking = true); - static bool getPointerMotionEvent(const vpImage &I, - vpImagePoint &ip); + static bool getKeyboardEvent(const vpImage &I, bool blocking = true); + static bool getKeyboardEvent(const vpImage &I, std::string &key, bool blocking = true); + static bool getKeyboardEvent(const vpImage &I, char *key, bool blocking = true); + static bool getPointerMotionEvent(const vpImage &I, vpImagePoint &ip); static bool getPointerPosition(const vpImage &I, vpImagePoint &ip); static void setBackground(const vpImage &I, const vpColor &color); static void setFont(const vpImage &I, const std::string &font); - static void setTitle(const vpImage &I, - const std::string &windowtitle); + static void setTitle(const vpImage &I, const std::string &windowtitle); static void setWindowPosition(const vpImage &I, int winx, int winy); //@} diff --git a/modules/core/include/visp3/core/vpDisplayException.h b/modules/core/include/visp3/core/vpDisplayException.h index 07879fd875..5f870f2ed2 100644 --- a/modules/core/include/visp3/core/vpDisplayException.h +++ b/modules/core/include/visp3/core/vpDisplayException.h @@ -94,10 +94,7 @@ class VISP_EXPORT vpDisplayException : public vpException va_end(args); } - vpDisplayException(const int id, const std::string &msg) - : vpException(id, msg) - { - } + vpDisplayException(const int id, const std::string &msg) : vpException(id, msg) {} explicit vpDisplayException(const int id) : vpException(id) {} }; diff --git a/modules/core/include/visp3/core/vpException.h b/modules/core/include/visp3/core/vpException.h index 6754ea19cf..015305973b 100644 --- a/modules/core/include/visp3/core/vpException.h +++ b/modules/core/include/visp3/core/vpException.h @@ -94,8 +94,8 @@ class VISP_EXPORT vpException : public std::exception divideByZeroError, dimensionError, fatalError, - badValue, /*!< Used to indicate that a value is not in the allowed range. - */ + badValue, /*!< Used to indicate that a value is not in the allowed range. + */ notInitialized /*!< Used to indicate that a parameter is not initialized. */ }; @@ -124,8 +124,7 @@ class VISP_EXPORT vpException : public std::exception //@} //! Print the error structure. - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpException &art); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpException &art); const char *what() const throw(); }; diff --git a/modules/core/include/visp3/core/vpExponentialMap.h b/modules/core/include/visp3/core/vpExponentialMap.h index b05a2a983c..661f792bde 100644 --- a/modules/core/include/visp3/core/vpExponentialMap.h +++ b/modules/core/include/visp3/core/vpExponentialMap.h @@ -92,10 +92,8 @@ class VISP_EXPORT vpExponentialMap { public: static vpHomogeneousMatrix direct(const vpColVector &v); - static vpHomogeneousMatrix direct(const vpColVector &v, - const double &delta_t); + 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); + static vpColVector inverse(const vpHomogeneousMatrix &M, const double &delta_t); }; #endif diff --git a/modules/core/include/visp3/core/vpFeatureDisplay.h b/modules/core/include/visp3/core/vpFeatureDisplay.h index 4dd751d097..f3f841388d 100644 --- a/modules/core/include/visp3/core/vpFeatureDisplay.h +++ b/modules/core/include/visp3/core/vpFeatureDisplay.h @@ -63,48 +63,30 @@ class VISP_EXPORT vpFeatureDisplay { public: - static void displayCylinder(double rho1, double theta1, double rho2, - double theta2, const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color = vpColor::green, + static void displayCylinder(double rho1, double theta1, double rho2, double theta2, const vpCameraParameters &cam, + const vpImage &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 &I, - const vpColor &color = vpColor::green, + static void displayCylinder(double rho1, double theta1, double rho2, double theta2, const vpCameraParameters &cam, + const vpImage &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 &I, - const vpColor &color = vpColor::green, + static void displayEllipse(double x, double y, double mu20, double mu11, double m02, const vpCameraParameters &cam, + const vpImage &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 &I, - const vpColor &color = vpColor::green, + static void displayEllipse(double x, double y, double mu20, double mu11, double m02, const vpCameraParameters &cam, + const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1); - static void displayLine(double rho, double theta, - const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color = vpColor::green, - unsigned int thickness = 1); - static void displayLine(double rho, double theta, - const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color = vpColor::green, - unsigned int thickness = 1); + static void displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage &I, + const vpColor &color = vpColor::green, unsigned int thickness = 1); + static void displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage &I, + const vpColor &color = vpColor::green, unsigned int thickness = 1); - static void displayPoint(double x, double y, const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color = vpColor::green, - unsigned int thickness = 1); - static void displayPoint(double x, double y, const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color = vpColor::green, - unsigned int thickness = 1); + static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage &I, + const vpColor &color = vpColor::green, unsigned int thickness = 1); + static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage &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 index cde0dbea06..f000c2c18e 100644 --- a/modules/core/include/visp3/core/vpForceTwistMatrix.h +++ b/modules/core/include/visp3/core/vpForceTwistMatrix.h @@ -91,21 +91,21 @@ becomes block diagonal. It allows than to compute the force/torque at point int main() { - vpForceTwistMatrix sFp; // Twist transformation matrix from sensor to probe -frame + // Twist transformation matrix from sensor to probe frame + vpForceTwistMatrix sFp; - vpHomogeneousMatrix sMp; // Force/torque sensor frame to probe frame -transformation + // Force/torque sensor frame to probe frame transformation + vpHomogeneousMatrix sMp; // ... 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 + // Force/torque skew in the probe frame: fx,fy,fz,tx,ty,tz + vpColVector p_H(6); // ... 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 + // Force/torque skew in the sensor frame: fx,fy,fz,tx,ty,tz + vpColVector s_H(6); // Compute the value of the force/torque in the sensor frame s_H = sFp * p_H; @@ -125,10 +125,9 @@ class VISP_EXPORT vpForceTwistMatrix : public vpArray2D // Construction from Translation and rotation (matrix parameterization) vpForceTwistMatrix(const vpTranslationVector &t, const vpRotationMatrix &R); // Construction from Translation and rotation (ThetaU parameterization) - vpForceTwistMatrix(const vpTranslationVector &t, - const vpThetaUVector &thetau); - vpForceTwistMatrix(const double tx, const double ty, const double tz, - const double tux, const double tuy, const double tuz); + vpForceTwistMatrix(const vpTranslationVector &t, const vpThetaUVector &thetau); + vpForceTwistMatrix(const double tx, const double ty, const double tz, const double tux, const double tuy, + const double tuz); vpForceTwistMatrix(const vpRotationMatrix &R); vpForceTwistMatrix(const vpThetaUVector &thetau); @@ -138,12 +137,9 @@ class VISP_EXPORT vpForceTwistMatrix : public vpArray2D */ virtual ~vpForceTwistMatrix(){}; - vpForceTwistMatrix buildFrom(const vpTranslationVector &t, - const vpRotationMatrix &R); - vpForceTwistMatrix buildFrom(const vpTranslationVector &t, - const vpThetaUVector &thetau); - vpForceTwistMatrix buildFrom(const vpHomogeneousMatrix &M, - bool full = true); + vpForceTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R); + vpForceTwistMatrix buildFrom(const vpTranslationVector &t, const vpThetaUVector &thetau); + vpForceTwistMatrix buildFrom(const vpHomogeneousMatrix &M, bool full = true); vpForceTwistMatrix buildFrom(const vpRotationMatrix &R); vpForceTwistMatrix buildFrom(const vpThetaUVector &thetau); @@ -159,22 +155,19 @@ class VISP_EXPORT vpForceTwistMatrix : public vpArray2D // 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; + 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 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")); + throw(vpException(vpException::fatalError, "Cannot resize a velocity twist matrix")); }; #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) diff --git a/modules/core/include/visp3/core/vpForwardProjection.h b/modules/core/include/visp3/core/vpForwardProjection.h index 92c3173f65..3ff442e961 100644 --- a/modules/core/include/visp3/core/vpForwardProjection.h +++ b/modules/core/include/visp3/core/vpForwardProjection.h @@ -114,8 +114,7 @@ class VISP_EXPORT vpForwardProjection : public vpTracker With this method, the vpTracker::cP public attribute is not updated. */ - virtual void changeFrame(const vpHomogeneousMatrix &cMo, - vpColVector &cP) = 0; + virtual void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) = 0; /*! Computes the features parameters in the camera frame (\e cP) thanks @@ -173,10 +172,8 @@ class VISP_EXPORT vpForwardProjection : public vpTracker \param color : The desired color to display the line in the image. \param thickness : Thickness of the feature representation. */ - virtual void display(const vpImage &I, - const vpCameraParameters &cam, - const vpColor &color = vpColor::green, - const unsigned int thickness = 1) = 0; + virtual void display(const vpImage &I, const vpCameraParameters &cam, + const vpColor &color = vpColor::green, const unsigned int thickness = 1) = 0; /*! @@ -197,11 +194,8 @@ class VISP_EXPORT vpForwardProjection : public vpTracker \param color : The desired color to display the line in the image. \param thickness : Thickness of the feature representation. */ - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const vpColor &color = vpColor::green, - const unsigned int thickness = 1) = 0; + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &color = vpColor::green, const unsigned int thickness = 1) = 0; virtual void print() const; @@ -214,10 +208,7 @@ class VISP_EXPORT vpForwardProjection : public vpTracker /*! Used for memory issue especially in the vpServo class. */ - typedef enum { - user, - vpDisplayForwardProjection - } vpForwardProjectionDeallocatorType; + typedef enum { user, vpDisplayForwardProjection } vpForwardProjectionDeallocatorType; private: vpForwardProjectionDeallocatorType deallocate; diff --git a/modules/core/include/visp3/core/vpFrameGrabberException.h b/modules/core/include/visp3/core/vpFrameGrabberException.h index dec798807c..d57766974b 100644 --- a/modules/core/include/visp3/core/vpFrameGrabberException.h +++ b/modules/core/include/visp3/core/vpFrameGrabberException.h @@ -76,11 +76,7 @@ class VISP_EXPORT vpFrameGrabberException : public vpException \brief Lists the possible error than can be emmited while calling vpFrameGrabber member */ - enum errorFrameGrabberCodeEnum { - settingError, - initializationError, - otherError - }; + enum errorFrameGrabberCodeEnum { settingError, initializationError, otherError }; public: vpFrameGrabberException(const int id, const char *format, ...) @@ -91,11 +87,7 @@ class VISP_EXPORT vpFrameGrabberException : public vpException setMessage(format, args); va_end(args); } - vpFrameGrabberException(const int id, const std::string &msg) - : vpException(id, msg) - { - ; - } + vpFrameGrabberException(const int id, const std::string &msg) : vpException(id, msg) { ; } explicit vpFrameGrabberException(const int id) : vpException(id) { ; } }; diff --git a/modules/core/include/visp3/core/vpGEMM.h b/modules/core/include/visp3/core/vpGEMM.h index 67397d968f..f05ffc5c1d 100644 --- a/modules/core/include/visp3/core/vpGEMM.h +++ b/modules/core/include/visp3/core/vpGEMM.h @@ -61,18 +61,14 @@ typedef enum { } vpGEMMmethod; template -inline void GEMMsize(const vpArray2D & /*A*/, - const vpArray2D & /*B*/, - unsigned int & /*Arows*/, unsigned int & /*Acols*/, - unsigned int & /*Brows*/, unsigned int & /*Bcols*/) +inline void GEMMsize(const vpArray2D & /*A*/, const vpArray2D & /*B*/, unsigned int & /*Arows*/, + unsigned int & /*Acols*/, unsigned int & /*Brows*/, unsigned int & /*Bcols*/) { } template <> -void inline GEMMsize<0>(const vpArray2D &A, - const vpArray2D &B, unsigned int &Arows, - unsigned int &Acols, unsigned int &Brows, - unsigned int &Bcols) +void inline GEMMsize<0>(const vpArray2D &A, const vpArray2D &B, unsigned int &Arows, + unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols) { Arows = A.getRows(); Acols = A.getCols(); @@ -81,10 +77,8 @@ void inline GEMMsize<0>(const vpArray2D &A, } template <> -inline void GEMMsize<1>(const vpArray2D &A, - const vpArray2D &B, unsigned int &Arows, - unsigned int &Acols, unsigned int &Brows, - unsigned int &Bcols) +inline void GEMMsize<1>(const vpArray2D &A, const vpArray2D &B, unsigned int &Arows, + unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols) { Arows = A.getCols(); Acols = A.getRows(); @@ -92,10 +86,8 @@ inline void GEMMsize<1>(const vpArray2D &A, Bcols = B.getCols(); } template <> -inline void GEMMsize<2>(const vpArray2D &A, - const vpArray2D &B, unsigned int &Arows, - unsigned int &Acols, unsigned int &Brows, - unsigned int &Bcols) +inline void GEMMsize<2>(const vpArray2D &A, const vpArray2D &B, unsigned int &Arows, + unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols) { Arows = A.getRows(); Acols = A.getCols(); @@ -103,10 +95,8 @@ inline void GEMMsize<2>(const vpArray2D &A, Bcols = B.getRows(); } template <> -inline void GEMMsize<3>(const vpArray2D &A, - const vpArray2D &B, unsigned int &Arows, - unsigned int &Acols, unsigned int &Brows, - unsigned int &Bcols) +inline void GEMMsize<3>(const vpArray2D &A, const vpArray2D &B, unsigned int &Arows, + unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols) { Arows = A.getCols(); Acols = A.getRows(); @@ -115,10 +105,8 @@ inline void GEMMsize<3>(const vpArray2D &A, } template <> -inline void GEMMsize<4>(const vpArray2D &A, - const vpArray2D &B, unsigned int &Arows, - unsigned int &Acols, unsigned int &Brows, - unsigned int &Bcols) +inline void GEMMsize<4>(const vpArray2D &A, const vpArray2D &B, unsigned int &Arows, + unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols) { Arows = A.getRows(); Acols = A.getCols(); @@ -127,10 +115,8 @@ inline void GEMMsize<4>(const vpArray2D &A, } template <> -inline void GEMMsize<5>(const vpArray2D &A, - const vpArray2D &B, unsigned int &Arows, - unsigned int &Acols, unsigned int &Brows, - unsigned int &Bcols) +inline void GEMMsize<5>(const vpArray2D &A, const vpArray2D &B, unsigned int &Arows, + unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols) { Arows = A.getCols(); Acols = A.getRows(); @@ -139,10 +125,8 @@ inline void GEMMsize<5>(const vpArray2D &A, } template <> -inline void GEMMsize<6>(const vpArray2D &A, - const vpArray2D &B, unsigned int &Arows, - unsigned int &Acols, unsigned int &Brows, - unsigned int &Bcols) +inline void GEMMsize<6>(const vpArray2D &A, const vpArray2D &B, unsigned int &Arows, + unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols) { Arows = A.getRows(); Acols = A.getCols(); @@ -151,10 +135,8 @@ inline void GEMMsize<6>(const vpArray2D &A, } template <> -inline void GEMMsize<7>(const vpArray2D &A, - const vpArray2D &B, unsigned int &Arows, - unsigned int &Acols, unsigned int &Brows, - unsigned int &Bcols) +inline void GEMMsize<7>(const vpArray2D &A, const vpArray2D &B, unsigned int &Arows, + unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols) { Arows = A.getCols(); Acols = A.getRows(); @@ -163,19 +145,15 @@ inline void GEMMsize<7>(const vpArray2D &A, } template -inline void -GEMM1(const unsigned int & /*Arows*/, const unsigned int & /*Brows*/, - const unsigned int & /*Bcols*/, const vpArray2D & /*A*/, - const vpArray2D & /*B*/, const double & /*alpha*/, - vpArray2D & /*D*/) +inline void GEMM1(const unsigned int & /*Arows*/, const unsigned int & /*Brows*/, const unsigned int & /*Bcols*/, + const vpArray2D & /*A*/, const vpArray2D & /*B*/, const double & /*alpha*/, + vpArray2D & /*D*/) { } template <> -inline void GEMM1<0>(const unsigned int &Arows, const unsigned int &Brows, - const unsigned int &Bcols, const vpArray2D &A, - const vpArray2D &B, const double &alpha, - vpArray2D &D) +inline void GEMM1<0>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, + const vpArray2D &A, const vpArray2D &B, const double &alpha, vpArray2D &D) { for (unsigned int r = 0; r < Arows; r++) for (unsigned int c = 0; c < Bcols; c++) { @@ -187,10 +165,8 @@ inline void GEMM1<0>(const unsigned int &Arows, const unsigned int &Brows, } template <> -inline void GEMM1<1>(const unsigned int &Arows, const unsigned int &Brows, - const unsigned int &Bcols, const vpArray2D &A, - const vpArray2D &B, const double &alpha, - vpArray2D &D) +inline void GEMM1<1>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, + const vpArray2D &A, const vpArray2D &B, const double &alpha, vpArray2D &D) { for (unsigned int r = 0; r < Arows; r++) for (unsigned int c = 0; c < Bcols; c++) { @@ -202,10 +178,8 @@ inline void GEMM1<1>(const unsigned int &Arows, const unsigned int &Brows, } template <> -inline void GEMM1<2>(const unsigned int &Arows, const unsigned int &Brows, - const unsigned int &Bcols, const vpArray2D &A, - const vpArray2D &B, const double &alpha, - vpArray2D &D) +inline void GEMM1<2>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, + const vpArray2D &A, const vpArray2D &B, const double &alpha, vpArray2D &D) { for (unsigned int r = 0; r < Arows; r++) for (unsigned int c = 0; c < Bcols; c++) { @@ -217,10 +191,8 @@ inline void GEMM1<2>(const unsigned int &Arows, const unsigned int &Brows, } template <> -inline void GEMM1<3>(const unsigned int &Arows, const unsigned int &Brows, - const unsigned int &Bcols, const vpArray2D &A, - const vpArray2D &B, const double &alpha, - vpArray2D &D) +inline void GEMM1<3>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, + const vpArray2D &A, const vpArray2D &B, const double &alpha, vpArray2D &D) { for (unsigned int r = 0; r < Arows; r++) for (unsigned int c = 0; c < Bcols; c++) { @@ -232,21 +204,16 @@ inline void GEMM1<3>(const unsigned int &Arows, const unsigned int &Brows, } template -inline void -GEMM2(const unsigned int & /*Arows*/, const unsigned int & /*Brows*/, - const unsigned int & /*Bcols*/, const vpArray2D & /*A*/, - const vpArray2D & /*B*/, const double & /*alpha*/, - const vpArray2D & /*C*/, const double & /*beta*/, - vpArray2D & /*D*/) +inline void GEMM2(const unsigned int & /*Arows*/, const unsigned int & /*Brows*/, const unsigned int & /*Bcols*/, + const vpArray2D & /*A*/, const vpArray2D & /*B*/, const double & /*alpha*/, + const vpArray2D & /*C*/, const double & /*beta*/, vpArray2D & /*D*/) { } template <> -inline void GEMM2<0>(const unsigned int &Arows, const unsigned int &Brows, - const unsigned int &Bcols, const vpArray2D &A, - const vpArray2D &B, const double &alpha, - const vpArray2D &C, const double &beta, - vpArray2D &D) +inline void GEMM2<0>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, + const vpArray2D &A, const vpArray2D &B, const double &alpha, + const vpArray2D &C, const double &beta, vpArray2D &D) { for (unsigned int r = 0; r < Arows; r++) for (unsigned int c = 0; c < Bcols; c++) { @@ -258,11 +225,9 @@ inline void GEMM2<0>(const unsigned int &Arows, const unsigned int &Brows, } template <> -inline void GEMM2<1>(const unsigned int &Arows, const unsigned int &Brows, - const unsigned int &Bcols, const vpArray2D &A, - const vpArray2D &B, const double &alpha, - const vpArray2D &C, const double &beta, - vpArray2D &D) +inline void GEMM2<1>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, + const vpArray2D &A, const vpArray2D &B, const double &alpha, + const vpArray2D &C, const double &beta, vpArray2D &D) { for (unsigned int r = 0; r < Arows; r++) for (unsigned int c = 0; c < Bcols; c++) { @@ -274,11 +239,9 @@ inline void GEMM2<1>(const unsigned int &Arows, const unsigned int &Brows, } template <> -inline void GEMM2<2>(const unsigned int &Arows, const unsigned int &Brows, - const unsigned int &Bcols, const vpArray2D &A, - const vpArray2D &B, const double &alpha, - const vpArray2D &C, const double &beta, - vpArray2D &D) +inline void GEMM2<2>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, + const vpArray2D &A, const vpArray2D &B, const double &alpha, + const vpArray2D &C, const double &beta, vpArray2D &D) { for (unsigned int r = 0; r < Arows; r++) for (unsigned int c = 0; c < Bcols; c++) { @@ -290,11 +253,9 @@ inline void GEMM2<2>(const unsigned int &Arows, const unsigned int &Brows, } template <> -inline void GEMM2<3>(const unsigned int &Arows, const unsigned int &Brows, - const unsigned int &Bcols, const vpArray2D &A, - const vpArray2D &B, const double &alpha, - const vpArray2D &C, const double &beta, - vpArray2D &D) +inline void GEMM2<3>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, + const vpArray2D &A, const vpArray2D &B, const double &alpha, + const vpArray2D &C, const double &beta, vpArray2D &D) { for (unsigned int r = 0; r < Arows; r++) for (unsigned int c = 0; c < Bcols; c++) { @@ -306,11 +267,9 @@ inline void GEMM2<3>(const unsigned int &Arows, const unsigned int &Brows, } template <> -inline void GEMM2<4>(const unsigned int &Arows, const unsigned int &Brows, - const unsigned int &Bcols, const vpArray2D &A, - const vpArray2D &B, const double &alpha, - const vpArray2D &C, const double &beta, - vpArray2D &D) +inline void GEMM2<4>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, + const vpArray2D &A, const vpArray2D &B, const double &alpha, + const vpArray2D &C, const double &beta, vpArray2D &D) { for (unsigned int r = 0; r < Arows; r++) for (unsigned int c = 0; c < Bcols; c++) { @@ -322,11 +281,9 @@ inline void GEMM2<4>(const unsigned int &Arows, const unsigned int &Brows, } template <> -inline void GEMM2<5>(const unsigned int &Arows, const unsigned int &Brows, - const unsigned int &Bcols, const vpArray2D &A, - const vpArray2D &B, const double &alpha, - const vpArray2D &C, const double &beta, - vpArray2D &D) +inline void GEMM2<5>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, + const vpArray2D &A, const vpArray2D &B, const double &alpha, + const vpArray2D &C, const double &beta, vpArray2D &D) { for (unsigned int r = 0; r < Arows; r++) for (unsigned int c = 0; c < Bcols; c++) { @@ -338,11 +295,9 @@ inline void GEMM2<5>(const unsigned int &Arows, const unsigned int &Brows, } template <> -inline void GEMM2<6>(const unsigned int &Arows, const unsigned int &Brows, - const unsigned int &Bcols, const vpArray2D &A, - const vpArray2D &B, const double &alpha, - const vpArray2D &C, const double &beta, - vpArray2D &D) +inline void GEMM2<6>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, + const vpArray2D &A, const vpArray2D &B, const double &alpha, + const vpArray2D &C, const double &beta, vpArray2D &D) { for (unsigned int r = 0; r < Arows; r++) for (unsigned int c = 0; c < Bcols; c++) { @@ -354,11 +309,9 @@ inline void GEMM2<6>(const unsigned int &Arows, const unsigned int &Brows, } template <> -inline void GEMM2<7>(const unsigned int &Arows, const unsigned int &Brows, - const unsigned int &Bcols, const vpArray2D &A, - const vpArray2D &B, const double &alpha, - const vpArray2D &C, const double &beta, - vpArray2D &D) +inline void GEMM2<7>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, + const vpArray2D &A, const vpArray2D &B, const double &alpha, + const vpArray2D &C, const double &beta, vpArray2D &D) { for (unsigned int r = 0; r < Arows; r++) for (unsigned int c = 0; c < Bcols; c++) { @@ -370,9 +323,8 @@ inline void GEMM2<7>(const unsigned int &Arows, const unsigned int &Brows, } template -inline void vpTGEMM(const vpArray2D &A, const vpArray2D &B, - const double &alpha, const vpArray2D &C, - const double &beta, vpArray2D &D) +inline void vpTGEMM(const vpArray2D &A, const vpArray2D &B, const double &alpha, + const vpArray2D &C, const double &beta, vpArray2D &D) { unsigned int Arows; unsigned int Acols; @@ -389,18 +341,14 @@ inline void vpTGEMM(const vpArray2D &A, const vpArray2D &B, } if (Acols != Brows) { - throw(vpException( - vpException::dimensionError, - "In vpGEMM, cannot multiply (%dx%d) matrix by (%dx%d) matrix", Arows, - Acols, Brows, Bcols)); + 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())); + throw(vpException(vpException::dimensionError, "In vpGEMM, cannot add resulting (%dx%d) matrix to (%dx%d) matrix", + Arows, Bcols, C.getRows(), C.getCols())); } GEMM2(Arows, Brows, Bcols, A, B, alpha, C, beta, D); @@ -442,10 +390,8 @@ inline void vpTGEMM(const vpArray2D &A, const vpArray2D &B, \relates vpArray2D */ -inline void vpGEMM(const vpArray2D &A, const vpArray2D &B, - const double &alpha, const vpArray2D &C, - const double &beta, vpArray2D &D, - const unsigned int &ops = 0) +inline void vpGEMM(const vpArray2D &A, const vpArray2D &B, const double &alpha, + const vpArray2D &C, const double &beta, vpArray2D &D, const unsigned int &ops = 0) { switch (ops) { case 0: @@ -473,8 +419,7 @@ inline void vpGEMM(const vpArray2D &A, const vpArray2D &B, vpTGEMM<7>(A, B, alpha, C, beta, D); break; default: - throw(vpException(vpException::functionNotImplementedError, - "Operation on vpGEMM not implemented")); + throw(vpException(vpException::functionNotImplementedError, "Operation on vpGEMM not implemented")); break; } } diff --git a/modules/core/include/visp3/core/vpGaussRand.h b/modules/core/include/visp3/core/vpGaussRand.h index 0cfd1d7102..7440f68b50 100644 --- a/modules/core/include/visp3/core/vpGaussRand.h +++ b/modules/core/include/visp3/core/vpGaussRand.h @@ -137,8 +137,7 @@ class VISP_EXPORT vpGaussRand : public vpUniRand \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) + vpGaussRand(const double sigma_val, const double mean_val, const long noise_seed = 0) : vpUniRand(noise_seed), mean(mean_val), sigma(sigma_val) { } diff --git a/modules/core/include/visp3/core/vpHistogram.h b/modules/core/include/visp3/core/vpHistogram.h index 82f08d49db..930ea82964 100644 --- a/modules/core/include/visp3/core/vpHistogram.h +++ b/modules/core/include/visp3/core/vpHistogram.h @@ -238,26 +238,19 @@ class VISP_EXPORT vpHistogram } }; - void calculate(const vpImage &I, - const unsigned int nbins = 256, - const unsigned int nbThreads = 1); + void calculate(const vpImage &I, const unsigned int nbins = 256, const unsigned int nbThreads = 1); - void display(const vpImage &I, - const vpColor &color = vpColor::white, - const unsigned int thickness = 2, + void display(const vpImage &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 &peaks); - unsigned getPeaks(unsigned char dist, vpHistogramPeak &peak1, - vpHistogramPeak &peak2); - bool getPeaks(unsigned char dist, vpHistogramPeak &peakl, - vpHistogramPeak &peakr, vpHistogramValey &valey); + unsigned getPeaks(unsigned char dist, vpHistogramPeak &peak1, vpHistogramPeak &peak2); + bool getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogramPeak &peakr, vpHistogramValey &valey); unsigned getValey(std::list &valey); - bool getValey(const vpHistogramPeak &peak1, const vpHistogramPeak &peak2, - vpHistogramValey &valey); - unsigned getValey(unsigned char dist, const vpHistogramPeak &peak, - vpHistogramValey &valeyl, vpHistogramValey &valeyr); + 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 &peaks); bool write(const std::string &filename); diff --git a/modules/core/include/visp3/core/vpHistogramPeak.h b/modules/core/include/visp3/core/vpHistogramPeak.h index c0461de3ba..2197215e97 100644 --- a/modules/core/include/visp3/core/vpHistogramPeak.h +++ b/modules/core/include/visp3/core/vpHistogramPeak.h @@ -139,8 +139,7 @@ class VISP_EXPORT vpHistogramPeak //--------------------------------- // Printing //--------------------------------- - friend VISP_EXPORT std::ostream &operator<<(std::ostream &s, - const vpHistogramPeak &p); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &s, const vpHistogramPeak &p); protected: unsigned char level; //! Gray level ot the peak. diff --git a/modules/core/include/visp3/core/vpHistogramValey.h b/modules/core/include/visp3/core/vpHistogramValey.h index e50117f4ed..59a837188e 100644 --- a/modules/core/include/visp3/core/vpHistogramValey.h +++ b/modules/core/include/visp3/core/vpHistogramValey.h @@ -65,8 +65,7 @@ class VISP_EXPORT vpHistogramValey : vpHistogramPeak public: vpHistogramValey() : vpHistogramPeak(){}; - vpHistogramValey(unsigned char lvl, unsigned val) - : vpHistogramPeak(lvl, val){}; + vpHistogramValey(unsigned char lvl, unsigned val) : vpHistogramPeak(lvl, val){}; vpHistogramValey(const vpHistogramValey &v) : vpHistogramPeak(v){}; @@ -140,8 +139,7 @@ class VISP_EXPORT vpHistogramValey : vpHistogramPeak //--------------------------------- // Printing //--------------------------------- - friend VISP_EXPORT std::ostream &operator<<(std::ostream &s, - const vpHistogramValey &v); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &s, const vpHistogramValey &v); }; /* diff --git a/modules/core/include/visp3/core/vpHomogeneousMatrix.h b/modules/core/include/visp3/core/vpHomogeneousMatrix.h index f435f9432d..5d157caf0a 100644 --- a/modules/core/include/visp3/core/vpHomogeneousMatrix.h +++ b/modules/core/include/visp3/core/vpHomogeneousMatrix.h @@ -94,16 +94,14 @@ class VISP_EXPORT vpHomogeneousMatrix : public vpArray2D public: vpHomogeneousMatrix(); vpHomogeneousMatrix(const vpHomogeneousMatrix &M); - vpHomogeneousMatrix(const vpTranslationVector &t, - const vpRotationMatrix &R); + vpHomogeneousMatrix(const vpTranslationVector &t, const vpRotationMatrix &R); vpHomogeneousMatrix(const vpTranslationVector &t, const vpThetaUVector &tu); - vpHomogeneousMatrix(const vpTranslationVector &t, - const vpQuaternionVector &q); + vpHomogeneousMatrix(const vpTranslationVector &t, const vpQuaternionVector &q); explicit vpHomogeneousMatrix(const vpPoseVector &p); explicit vpHomogeneousMatrix(const std::vector &v); explicit vpHomogeneousMatrix(const std::vector &v); - vpHomogeneousMatrix(const double tx, const double ty, const double tz, - const double tux, const double tuy, const double tuz); + vpHomogeneousMatrix(const double tx, const double ty, const double tz, const double tux, const double tuy, + const double tuz); /*! Destructor. */ @@ -115,8 +113,8 @@ class VISP_EXPORT vpHomogeneousMatrix : public vpArray2D void buildFrom(const vpPoseVector &p); void buildFrom(const std::vector &v); void buildFrom(const std::vector &v); - void buildFrom(const double tx, const double ty, const double tz, - const double tux, const double tuy, const double tuz); + void buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, + const double tuz); void convert(std::vector &M); void convert(std::vector &M); @@ -169,14 +167,12 @@ class VISP_EXPORT vpHomogeneousMatrix : public vpArray2D 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 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")); + throw(vpException(vpException::fatalError, "Cannot resize an homogeneous matrix")); }; #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) diff --git a/modules/core/include/visp3/core/vpImage.h b/modules/core/include/visp3/core/vpImage.h index 421305b469..598a5de4ae 100644 --- a/modules/core/include/visp3/core/vpImage.h +++ b/modules/core/include/visp3/core/vpImage.h @@ -113,12 +113,10 @@ value = I[i][j]; // Here we will get the pixel value at position (101, 80) */ // Ref: http://en.cppreference.com/w/cpp/language/friend#Template_friends -template -class vpImage; // forward declare to make function declaration possible +template class vpImage; // forward declare to make function declaration possible // declarations -template -std::ostream &operator<<(std::ostream &, const vpImage &); +template std::ostream &operator<<(std::ostream &, const vpImage &); std::ostream &operator<<(std::ostream &, const vpImage &); std::ostream &operator<<(std::ostream &, const vpImage &); @@ -148,8 +146,7 @@ template class vpImage //! 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); + vpImage(Type *const array, const unsigned int height, const unsigned int width, const bool copyData = false); //! destructor virtual ~vpImage(); @@ -239,8 +236,7 @@ template class vpImage //! 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); + void init(Type *const array, const unsigned int height, const unsigned int width, const bool copyData = false); void insert(const vpImage &src, const vpImagePoint &topLeft); //------------------------------------------------------------------ @@ -261,20 +257,13 @@ template class vpImage \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]; - } + inline Type operator()(const unsigned int i, const unsigned int j) const { return bitmap[i * width + j]; } /*! Set the value \e v of an image point with coordinates (i, j), with i the row position and j the column position. */ - inline void operator()(const unsigned int i, const unsigned int j, - const Type &v) - { - bitmap[i * width + j] = v; - } + 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. @@ -319,8 +308,7 @@ template class vpImage bool operator==(const vpImage &I); bool operator!=(const vpImage &I); friend std::ostream &operator<<<>(std::ostream &s, const vpImage &I); - friend std::ostream &operator<<(std::ostream &s, - const vpImage &I); + friend std::ostream &operator<<(std::ostream &s, const vpImage &I); friend std::ostream &operator<<(std::ostream &s, const vpImage &I); friend std::ostream &operator<<(std::ostream &s, const vpImage &I); friend std::ostream &operator<<(std::ostream &s, const vpImage &I); @@ -338,8 +326,7 @@ template class vpImage void sub(const vpImage &B, vpImage &C); void sub(const vpImage &A, const vpImage &B, vpImage &C); - void subsample(unsigned int v_scale, unsigned int h_scale, - vpImage &sampled) const; + void subsample(unsigned int v_scale, unsigned int h_scale, vpImage &sampled) const; friend void swap<>(vpImage &first, vpImage &second); @@ -352,8 +339,7 @@ template class vpImage Type **row; ///! points the row pointer array }; -template -std::ostream &operator<<(std::ostream &s, const vpImage &I) +template std::ostream &operator<<(std::ostream &s, const vpImage &I) { if (I.bitmap == NULL) { return s; @@ -376,8 +362,7 @@ std::ostream &operator<<(std::ostream &s, const vpImage &I) return s; } -inline std::ostream &operator<<(std::ostream &s, - const vpImage &I) +inline std::ostream &operator<<(std::ostream &s, const vpImage &I) { if (I.bitmap == NULL) { return s; @@ -436,8 +421,7 @@ inline std::ostream &operator<<(std::ostream &s, const vpImage &I) } std::ios_base::fmtflags original_flags = s.flags(); - s.precision( - 9); // http://en.cppreference.com/w/cpp/types/numeric_limits/max_digits10 + s.precision(9); // http://en.cppreference.com/w/cpp/types/numeric_limits/max_digits10 for (unsigned int i = 0; i < I.getHeight(); i++) { for (unsigned int j = 0; j < I.getWidth() - 1; j++) { @@ -464,8 +448,7 @@ inline std::ostream &operator<<(std::ostream &s, const vpImage &I) } std::ios_base::fmtflags original_flags = s.flags(); - s.precision( - 17); // http://en.cppreference.com/w/cpp/types/numeric_limits/max_digits10 + s.precision(17); // http://en.cppreference.com/w/cpp/types/numeric_limits/max_digits10 for (unsigned int i = 0; i < I.getHeight(); i++) { for (unsigned int j = 0; j < I.getWidth() - 1; j++) { @@ -495,15 +478,10 @@ struct ImageLut_Param_t { unsigned char m_lut[256]; unsigned char *m_bitmap; - ImageLut_Param_t() - : m_start_index(0), m_end_index(0), m_lut(), m_bitmap(NULL) - { - } + ImageLut_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_bitmap(NULL) {} - ImageLut_Param_t(const unsigned int start_index, - const unsigned int end_index, unsigned char *bitmap) - : m_start_index(start_index), m_end_index(end_index), m_lut(), - m_bitmap(bitmap) + ImageLut_Param_t(const unsigned int start_index, const unsigned int end_index, unsigned char *bitmap) + : m_start_index(start_index), m_end_index(end_index), m_lut(), m_bitmap(bitmap) { } }; @@ -568,23 +546,17 @@ struct ImageLutRGBa_Param_t { vpRGBa m_lut[256]; unsigned char *m_bitmap; - ImageLutRGBa_Param_t() - : m_start_index(0), m_end_index(0), m_lut(), m_bitmap(NULL) - { - } + ImageLutRGBa_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_bitmap(NULL) {} - ImageLutRGBa_Param_t(const unsigned int start_index, - const unsigned int end_index, unsigned char *bitmap) - : m_start_index(start_index), m_end_index(end_index), m_lut(), - m_bitmap(bitmap) + ImageLutRGBa_Param_t(const unsigned int start_index, const unsigned int end_index, unsigned char *bitmap) + : m_start_index(start_index), m_end_index(end_index), m_lut(), m_bitmap(bitmap) { } }; vpThread::Return performLutRGBaThread(vpThread::Args args) { - ImageLutRGBa_Param_t *imageLut_param = - static_cast(args); + ImageLutRGBa_Param_t *imageLut_param = static_cast(args); unsigned int start_index = imageLut_param->m_start_index; unsigned int end_index = imageLut_param->m_end_index; @@ -649,8 +621,7 @@ vpThread::Return performLutRGBaThread(vpThread::Args args) \sa vpImage::init(h, w) */ -template -void vpImage::init(unsigned int h, unsigned int w, Type value) +template void vpImage::init(unsigned int h, unsigned int w, Type value) { init(h, w); @@ -702,15 +673,13 @@ template void vpImage::init(unsigned int h, unsigned int w) bitmap = new Type[npixels]; if (bitmap == NULL) { - throw(vpException(vpException::memoryAllocationError, - "cannot allocate bitmap ")); + throw(vpException(vpException::memoryAllocationError, "cannot allocate bitmap ")); } if (row == NULL) row = new Type *[height]; if (row == NULL) { - throw(vpException(vpException::memoryAllocationError, - "cannot allocate row ")); + throw(vpException(vpException::memoryAllocationError, "cannot allocate row ")); } unsigned int i; @@ -732,8 +701,7 @@ template void vpImage::init(unsigned int h, unsigned int w) \exception vpException::memoryAllocationError */ template -void vpImage::init(Type *const array, const unsigned int h, - const unsigned int w, const bool copyData) +void vpImage::init(Type *const array, const unsigned int h, const unsigned int w, const bool copyData) { if (h != this->height) { if (row != NULL) { @@ -743,8 +711,7 @@ void vpImage::init(Type *const array, const unsigned int h, } // Delete bitmap if copyData==false, otherwise only if the dimension differs - if ((copyData && ((h != this->height) || (w != this->width))) || - !copyData) { + if ((copyData && ((h != this->height) || (w != this->width))) || !copyData) { if (bitmap != NULL) { delete[] bitmap; bitmap = NULL; @@ -761,8 +728,7 @@ void vpImage::init(Type *const array, const unsigned int h, bitmap = new Type[npixels]; if (bitmap == NULL) { - throw(vpException(vpException::memoryAllocationError, - "cannot allocate bitmap ")); + throw(vpException(vpException::memoryAllocationError, "cannot allocate bitmap ")); } // Copy the image data @@ -775,8 +741,7 @@ void vpImage::init(Type *const array, const unsigned int h, if (row == NULL) row = new Type *[height]; if (row == NULL) { - throw(vpException(vpException::memoryAllocationError, - "cannot allocate row ")); + throw(vpException(vpException::memoryAllocationError, "cannot allocate row ")); } for (unsigned int i = 0; i < height; i++) { @@ -849,8 +814,7 @@ vpImage::vpImage(unsigned int h, unsigned int w, Type value) \sa vpImage::init(array, height, width) */ template -vpImage::vpImage(Type *const array, const unsigned int h, - const unsigned int w, const bool copyData) +vpImage::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) { init(array, h, w, copyData); @@ -865,9 +829,7 @@ vpImage::vpImage(Type *const array, const unsigned int h, \sa vpImage::resize(height, width) for memory allocation */ -template -vpImage::vpImage() - : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL) +template vpImage::vpImage() : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL) { } @@ -891,11 +853,7 @@ vpImage::vpImage() \sa init(unsigned int, unsigned int) */ -template -void vpImage::resize(unsigned int h, unsigned int w) -{ - init(h, w); -} +template void vpImage::resize(unsigned int h, unsigned int w) { init(h, w); } /*! \brief resize the image : Image initialization @@ -916,11 +874,7 @@ void vpImage::resize(unsigned int h, unsigned int w) \sa init(unsigned int, unsigned int) */ -template -void vpImage::resize(unsigned int h, unsigned int w, const Type &val) -{ - init(h, w, val); -} +template void vpImage::resize(unsigned int h, unsigned int w, const Type &val) { init(h, w, val); } /*! \brief Destructor : Memory de-allocation @@ -959,8 +913,7 @@ template vpImage::~vpImage() { destroy(); } Copy constructor */ template -vpImage::vpImage(const vpImage &I) - : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL) +vpImage::vpImage(const vpImage &I) : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL) { resize(I.getHeight(), I.getWidth()); memcpy(bitmap, I.bitmap, I.npixels * sizeof(Type)); @@ -972,8 +925,7 @@ vpImage::vpImage(const vpImage &I) */ template vpImage::vpImage(vpImage &&I) - : bitmap(I.bitmap), display(I.display), npixels(I.npixels), width(I.width), - height(I.height), row(I.row) + : bitmap(I.bitmap), display(I.display), npixels(I.npixels), width(I.width), height(I.height), row(I.row) { I.bitmap = NULL; I.display = NULL; @@ -1019,8 +971,7 @@ template Type vpImage::getMinValue() const \sa getMaxValue() \sa getMinValue() */ -template -void vpImage::getMinMaxValue(Type &min, Type &max) const +template void vpImage::getMinMaxValue(Type &min, Type &max) const { min = max = bitmap[0]; for (unsigned int i = 0; i < npixels; i++) { @@ -1034,8 +985,7 @@ void vpImage::getMinMaxValue(Type &min, Type &max) const /*! \brief Copy operator */ -template -vpImage &vpImage::operator=(vpImage other) +template vpImage &vpImage::operator=(vpImage other) { swap(*this, other); // Swap back display pointer if it was not null @@ -1131,8 +1081,7 @@ int main() \sa sub(const vpImage &, const vpImage &, vpImage &) to avoid matrix allocation for each use. */ -template -vpImage vpImage::operator-(const vpImage &B) +template vpImage vpImage::operator-(const vpImage &B) { vpImage C; sub(*this, B, C); @@ -1150,9 +1099,7 @@ vpImage vpImage::operator-(const vpImage &B) \param topLeft : Upper/left coordinates in the image where the image \e src is inserted in the destination image. */ -template -void vpImage::insert(const vpImage &src, - const vpImagePoint &topLeft) +template void vpImage::insert(const vpImage &src, const vpImagePoint &topLeft) { int itl = (int)topLeft.get_i(); int jtl = (int)topLeft.get_j(); @@ -1193,8 +1140,7 @@ void vpImage::insert(const vpImage &src, for (int i = 0; i < hsize; i++) { Type *srcBitmap = src.bitmap + ((src_ibegin + i) * src_w + src_jbegin); - Type *destBitmap = - this->bitmap + ((dest_ibegin + i) * dest_w + dest_jbegin); + Type *destBitmap = this->bitmap + ((dest_ibegin + i) * dest_w + dest_jbegin); memcpy(destBitmap, srcBitmap, (size_t)wsize * sizeof(Type)); } @@ -1230,8 +1176,7 @@ void vpImage::insert(const vpImage &src, \sa subsample() */ -template -void vpImage::halfSizeImage(vpImage &res) const +template void vpImage::halfSizeImage(vpImage &res) const { unsigned int h = height / 2; unsigned int w = width / 2; @@ -1259,8 +1204,7 @@ void vpImage::halfSizeImage(vpImage &res) const \endcode */ template -void vpImage::subsample(unsigned int v_scale, unsigned int h_scale, - vpImage &sampled) const +void vpImage::subsample(unsigned int v_scale, unsigned int h_scale, vpImage &sampled) const { unsigned int h = height / v_scale; unsigned int w = width / h_scale; @@ -1294,8 +1238,7 @@ void vpImage::subsample(unsigned int v_scale, unsigned int h_scale, */ -template -void vpImage::quarterSizeImage(vpImage &res) const +template void vpImage::quarterSizeImage(vpImage &res) const { unsigned int h = height / 4; unsigned int w = width / 4; @@ -1360,22 +1303,18 @@ template void vpImage::doubleSizeImage(vpImage &res) // 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])); + 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])); + 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])); + 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])); } /*! @@ -1405,8 +1344,7 @@ template Type vpImage::getValue(double i, double j) const jround = (unsigned int)floor(j); if (iround >= height || jround >= width) { - throw(vpException(vpImageException::notInTheImage, - "Pixel outside the image")); + throw(vpException(vpImageException::notInTheImage, "Pixel outside the image")); } if (i > height - 1) @@ -1425,12 +1363,8 @@ template Type vpImage::getValue(double i, double j) const 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; + 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 (Type)vpMath::round(value); } @@ -1460,8 +1394,7 @@ template <> inline double vpImage::getValue(double i, double j) const jround = (unsigned int)floor(j); if (iround >= height || jround >= width) { - throw(vpException(vpImageException::notInTheImage, - "Pixel outside the image")); + throw(vpException(vpImageException::notInTheImage, "Pixel outside the image")); } if (i > height - 1) @@ -1480,12 +1413,8 @@ template <> inline double vpImage::getValue(double i, double j) const 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; + 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; } @@ -1498,8 +1427,7 @@ template <> inline vpRGBa vpImage::getValue(double i, double j) const jround = (unsigned int)floor(j); if (iround >= height || jround >= width) { - throw(vpException(vpImageException::notInTheImage, - "Pixel outside the image")); + throw(vpException(vpImageException::notInTheImage, "Pixel outside the image")); } if (i > height - 1) @@ -1518,26 +1446,13 @@ template <> inline vpRGBa vpImage::getValue(double i, double j) const 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), + 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)); } @@ -1558,8 +1473,7 @@ closest pixel. of the image. */ -template -inline Type vpImage::getValue(vpImagePoint &ip) const +template inline Type vpImage::getValue(vpImagePoint &ip) const { unsigned int iround, jround; double rfrac, cfrac; @@ -1568,8 +1482,7 @@ inline Type vpImage::getValue(vpImagePoint &ip) const jround = (unsigned int)floor(ip.get_j()); if (iround >= height || jround >= width) { - throw(vpException(vpImageException::notInTheImage, - "Pixel outside the image")); + throw(vpException(vpImageException::notInTheImage, "Pixel outside the image")); } if (ip.get_i() > height - 1) @@ -1588,12 +1501,8 @@ inline Type vpImage::getValue(vpImagePoint &ip) const 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; + 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 (Type)vpMath::round(value); } @@ -1606,8 +1515,7 @@ template <> inline double vpImage::getValue(vpImagePoint &ip) const jround = (unsigned int)floor(ip.get_j()); if (iround >= height || jround >= width) { - throw(vpException(vpImageException::notInTheImage, - "Pixel outside the image")); + throw(vpException(vpImageException::notInTheImage, "Pixel outside the image")); } if (ip.get_i() > height - 1) @@ -1626,12 +1534,8 @@ template <> inline double vpImage::getValue(vpImagePoint &ip) const 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; + 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; } @@ -1644,8 +1548,7 @@ template <> inline vpRGBa vpImage::getValue(vpImagePoint &ip) const jround = (unsigned int)floor(ip.get_j()); if (iround >= height || jround >= width) { - throw(vpException(vpImageException::notInTheImage, - "Pixel outside the image")); + throw(vpException(vpImageException::notInTheImage, "Pixel outside the image")); } if (ip.get_i() > height - 1) @@ -1664,26 +1567,13 @@ template <> inline vpRGBa vpImage::getValue(vpImagePoint &ip) const 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), + 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)); } @@ -1716,23 +1606,19 @@ int main() \sa operator-() */ -template -void vpImage::sub(const vpImage &B, vpImage &C) +template void vpImage::sub(const vpImage &B, vpImage &C) { try { - if ((this->getHeight() != C.getHeight()) || - (this->getWidth() != C.getWidth())) + if ((this->getHeight() != C.getHeight()) || (this->getWidth() != C.getWidth())) C.resize(this->getHeight(), this->getWidth()); } catch (vpException &me) { std::cout << me << std::endl; throw; } - if ((this->getWidth() != B.getWidth()) || - (this->getHeight() != B.getHeight())) { - throw(vpException(vpException::memoryAllocationError, - "vpImage mismatch in vpImage/vpImage substraction ")); + if ((this->getWidth() != B.getWidth()) || (this->getHeight() != B.getHeight())) { + throw(vpException(vpException::memoryAllocationError, "vpImage mismatch in vpImage/vpImage substraction ")); } for (unsigned int i = 0; i < this->getWidth() * this->getHeight(); i++) { @@ -1751,9 +1637,7 @@ void vpImage::sub(const vpImage &B, vpImage &C) \sa operator-() */ -template -void vpImage::sub(const vpImage &A, const vpImage &B, - vpImage &C) +template void vpImage::sub(const vpImage &A, const vpImage &B, vpImage &C) { try { @@ -1765,8 +1649,7 @@ void vpImage::sub(const vpImage &A, const vpImage &B, } if ((A.getWidth() != B.getWidth()) || (A.getHeight() != B.getHeight())) { - throw(vpException(vpException::memoryAllocationError, - "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++) { @@ -1784,8 +1667,7 @@ void vpImage::sub(const vpImage &A, const vpImage &B, const unsigned int) */ -template -void vpImage::performLut(const Type (&)[256], const unsigned int) +template void vpImage::performLut(const Type (&)[256], const unsigned int) { std::cerr << "Not implemented !" << std::endl; } @@ -1799,9 +1681,7 @@ void vpImage::performLut(const Type (&)[256], const unsigned int) the computation. */ template <> -inline void -vpImage::performLut(const unsigned char (&lut)[256], - const unsigned int nbThreads) +inline void vpImage::performLut(const unsigned char (&lut)[256], const unsigned int nbThreads) { unsigned int size = getWidth() * getHeight(); unsigned char *ptrStart = (unsigned char *)bitmap; @@ -1843,15 +1723,13 @@ vpImage::performLut(const unsigned char (&lut)[256], end_index = start_index + last_step; } - ImageLut_Param_t *imageLut_param = - new ImageLut_Param_t(start_index, end_index, bitmap); + ImageLut_Param_t *imageLut_param = new ImageLut_Param_t(start_index, end_index, bitmap); memcpy(imageLut_param->m_lut, lut, 256 * sizeof(unsigned char)); imageLutParams.push_back(imageLut_param); // Start the threads - vpThread *imageLut_thread = new vpThread( - (vpThread::Fn)performLutThread, (vpThread::Args)imageLut_param); + vpThread *imageLut_thread = new vpThread((vpThread::Fn)performLutThread, (vpThread::Args)imageLut_param); threadpool.push_back(imageLut_thread); } @@ -1880,9 +1758,7 @@ vpImage::performLut(const unsigned char (&lut)[256], intensity to his new value. \param nbThreads : Number of threads to use for the computation. */ -template <> -inline void vpImage::performLut(const vpRGBa (&lut)[256], - const unsigned int nbThreads) +template <> inline void vpImage::performLut(const vpRGBa (&lut)[256], const unsigned int nbThreads) { unsigned int size = getWidth() * getHeight(); unsigned char *ptrStart = (unsigned char *)bitmap; @@ -1931,15 +1807,13 @@ inline void vpImage::performLut(const vpRGBa (&lut)[256], end_index = start_index + last_step; } - ImageLutRGBa_Param_t *imageLut_param = new ImageLutRGBa_Param_t( - start_index, end_index, (unsigned char *)bitmap); + ImageLutRGBa_Param_t *imageLut_param = new ImageLutRGBa_Param_t(start_index, end_index, (unsigned char *)bitmap); memcpy(imageLut_param->m_lut, lut, 256 * sizeof(vpRGBa)); imageLutParams.push_back(imageLut_param); // Start the threads - vpThread *imageLut_thread = new vpThread( - (vpThread::Fn)performLutRGBaThread, (vpThread::Args)imageLut_param); + vpThread *imageLut_thread = new vpThread((vpThread::Fn)performLutRGBaThread, (vpThread::Args)imageLut_param); threadpool.push_back(imageLut_thread); } diff --git a/modules/core/include/visp3/core/vpImageConvert.h b/modules/core/include/visp3/core/vpImageConvert.h index 61a6bd2bff..65165748a9 100644 --- a/modules/core/include/visp3/core/vpImageConvert.h +++ b/modules/core/include/visp3/core/vpImageConvert.h @@ -102,87 +102,60 @@ class VISP_EXPORT vpImageConvert { public: - static void createDepthHistogram(const vpImage &src_depth, - vpImage &dest_rgba); - static void createDepthHistogram(const vpImage &src_depth, - vpImage &dest_depth); - static void convert(const vpImage &src, - vpImage &dest); - static void convert(const vpImage &src, - vpImage &dest); - - static void convert(const vpImage &src, - vpImage &dest); - static void convert(const vpImage &src, - vpImage &dest); - - static void convert(const vpImage &src, - vpImage &dest); - static void convert(const vpImage &src, - vpImage &dest); - - static void convert(const vpImage &src, - vpImage &dest); - static void convert(const vpImage &src, - vpImage &dest); + static void createDepthHistogram(const vpImage &src_depth, vpImage &dest_rgba); + static void createDepthHistogram(const vpImage &src_depth, vpImage &dest_depth); + static void convert(const vpImage &src, vpImage &dest); + static void convert(const vpImage &src, vpImage &dest); + + static void convert(const vpImage &src, vpImage &dest); + static void convert(const vpImage &src, vpImage &dest); + + static void convert(const vpImage &src, vpImage &dest); + static void convert(const vpImage &src, vpImage &dest); + + static void convert(const vpImage &src, vpImage &dest); + static void convert(const vpImage &src, vpImage &dest); /*! Make a copy of an image. \param src : source image. \param dest : destination image. */ - template - static void convert(const vpImage &src, vpImage &dest) - { - dest = src; - } + template static void convert(const vpImage &src, vpImage &dest) { dest = src; } #ifdef VISP_HAVE_OPENCV // Deprecated: will be removed with OpenCV transcient from C to C++ api - static void convert(const IplImage *src, vpImage &dest, - bool flip = false); - static void convert(const IplImage *src, vpImage &dest, - bool flip = false); + static void convert(const IplImage *src, vpImage &dest, bool flip = false); + static void convert(const IplImage *src, vpImage &dest, bool flip = false); static void convert(const vpImage &src, IplImage *&dest); static void convert(const vpImage &src, IplImage *&dest); #if VISP_HAVE_OPENCV_VERSION >= 0x020100 - static void convert(const cv::Mat &src, vpImage &dest, - const bool flip = false); - static void convert(const cv::Mat &src, vpImage &dest, - const bool flip = false); + static void convert(const cv::Mat &src, vpImage &dest, const bool flip = false); + static void convert(const cv::Mat &src, vpImage &dest, const bool flip = false); static void convert(const vpImage &src, cv::Mat &dest); - static void convert(const vpImage &src, cv::Mat &dest, - const bool copyData = true); + static void convert(const vpImage &src, cv::Mat &dest, const bool copyData = true); #endif #endif #ifdef VISP_HAVE_YARP - static void convert(const vpImage &src, - yarp::sig::ImageOf *dest, + static void convert(const vpImage &src, yarp::sig::ImageOf *dest, const bool copyData = true); - static void convert(const yarp::sig::ImageOf *src, - vpImage &dest, + static void convert(const yarp::sig::ImageOf *src, vpImage &dest, const bool copyData = true); - static void convert(const vpImage &src, - yarp::sig::ImageOf *dest, + static void convert(const vpImage &src, yarp::sig::ImageOf *dest, + const bool copyData = true); + static void convert(const yarp::sig::ImageOf *src, vpImage &dest, const bool copyData = true); - static void convert(const yarp::sig::ImageOf *src, - vpImage &dest, const bool copyData = true); - static void convert(const vpImage &src, - yarp::sig::ImageOf *dest); - static void convert(const yarp::sig::ImageOf *src, - vpImage &dest); + static void convert(const vpImage &src, yarp::sig::ImageOf *dest); + static void convert(const yarp::sig::ImageOf *src, vpImage &dest); #endif - static void split(const vpImage &src, vpImage *pR, - vpImage *pG, vpImage *pB, - vpImage *pa = NULL); + static void split(const vpImage &src, vpImage *pR, vpImage *pG, + vpImage *pB, vpImage *pa = NULL); - static void merge(const vpImage *R, - const vpImage *G, - const vpImage *B, + static void merge(const vpImage *R, const vpImage *G, const vpImage *B, const vpImage *a, vpImage &RGBa); /*! @@ -201,9 +174,8 @@ class VISP_EXPORT vpImageConvert \f] */ - static inline void YUVToRGB(unsigned char y, unsigned char u, - unsigned char v, unsigned char &r, - unsigned char &g, unsigned char &b) + 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)); @@ -221,128 +193,80 @@ class VISP_EXPORT vpImageConvert 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 *rgb, unsigned char *rgba, - unsigned int width, unsigned int height, + 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 *rgb, unsigned char *rgba, unsigned int width, unsigned int height, bool flip = false); - static void RGBToGrey(unsigned char *rgb, unsigned char *grey, - unsigned int width, unsigned int height, + static void RGBToGrey(unsigned char *rgb, 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 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, + 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, + 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, + 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 RGBaToHSV(const unsigned char *rgba, double *hue, - double *saturation, double *value, + 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, + 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, + 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, + 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, 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, + 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, + 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; diff --git a/modules/core/include/visp3/core/vpImageException.h b/modules/core/include/visp3/core/vpImageException.h index e8f9c70307..2ed86730e1 100644 --- a/modules/core/include/visp3/core/vpImageException.h +++ b/modules/core/include/visp3/core/vpImageException.h @@ -94,11 +94,7 @@ class VISP_EXPORT vpImageException : public vpException setMessage(format, args); va_end(args); } - vpImageException(const int id, const std::string &msg) - : vpException(id, msg) - { - ; - } + vpImageException(const int id, const std::string &msg) : vpException(id, msg) { ; } explicit vpImageException(const int id) : vpException(id) { ; } }; diff --git a/modules/core/include/visp3/core/vpImageFilter.h b/modules/core/include/visp3/core/vpImageFilter.h index 973a67685c..6e10e5cc09 100644 --- a/modules/core/include/visp3/core/vpImageFilter.h +++ b/modules/core/include/visp3/core/vpImageFilter.h @@ -67,11 +67,8 @@ class VISP_EXPORT vpImageFilter { public: #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100) - static void canny(const vpImage &I, - vpImage &Ic, - const unsigned int gaussianFilterSize, - const double thresholdCanny, - const unsigned int apertureSobel); + static void canny(const vpImage &I, vpImage &Ic, const unsigned int gaussianFilterSize, + const double thresholdCanny, const unsigned int apertureSobel); #endif /*! @@ -81,12 +78,9 @@ class VISP_EXPORT vpImageFilter \param r : coordinates (row) of the pixel \param c : coordinates (column) of the pixel */ - template - static double derivativeFilterX(const vpImage &I, const unsigned int r, - const unsigned int c) + template static double derivativeFilterX(const vpImage &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]) + + 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; } @@ -98,12 +92,9 @@ class VISP_EXPORT vpImageFilter \param r : coordinates (row) of the pixel \param c : coordinates (column) of the pixel */ - template - static double derivativeFilterY(const vpImage &I, const unsigned int r, - const unsigned int c) + template static double derivativeFilterY(const vpImage &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]) + + 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; } @@ -122,8 +113,7 @@ class VISP_EXPORT vpImageFilter */ template - static double derivativeFilterX(const vpImage &I, const unsigned int r, - const unsigned int c, const double *filter, + static double derivativeFilterX(const vpImage &I, const unsigned int r, const unsigned int c, const double *filter, const unsigned int size) { unsigned int i; @@ -150,8 +140,7 @@ class VISP_EXPORT vpImageFilter \sa vpImageFilter::getGaussianDerivativeKernel() */ template - static double derivativeFilterY(const vpImage &I, const unsigned int r, - const unsigned int c, const double *filter, + static double derivativeFilterY(const vpImage &I, const unsigned int r, const unsigned int c, const double *filter, const unsigned int size) { unsigned int i; @@ -165,49 +154,34 @@ class VISP_EXPORT vpImageFilter return result; } - static void filter(const vpImage &I, vpImage &Iu, - vpImage &Iv, const vpMatrix &M, + static void filter(const vpImage &I, vpImage &Iu, vpImage &Iv, const vpMatrix &M, const bool convolve = false); - static void filter(const vpImage &I, vpImage &If, - const vpMatrix &M, const bool convolve = false); + static void filter(const vpImage &I, vpImage &If, const vpMatrix &M, + const bool convolve = false); - static void sepFilter(const vpImage &I, vpImage &If, - const vpColVector &kernelH, + static void sepFilter(const vpImage &I, vpImage &If, const vpColVector &kernelH, const vpColVector &kernelV); - static void filter(const vpImage &I, vpImage &GI, - const double *filter, unsigned int size); - static void filter(const vpImage &I, vpImage &GI, - const double *filter, unsigned int size); + static void filter(const vpImage &I, vpImage &GI, const double *filter, unsigned int size); + static void filter(const vpImage &I, vpImage &GI, const double *filter, unsigned int size); - static inline unsigned char - filterGaussXPyramidal(const vpImage &I, unsigned int i, - unsigned int j) + static inline unsigned char filterGaussXPyramidal(const vpImage &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]) / + 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 &I, unsigned int i, - unsigned int j) + static inline unsigned char filterGaussYPyramidal(const vpImage &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]) / + 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 &I, vpImage &dIx, - const double *filter, unsigned int size); - static void filterX(const vpImage &I, vpImage &dIx, - const double *filter, unsigned int size); + static void filterX(const vpImage &I, vpImage &dIx, const double *filter, unsigned int size); + static void filterX(const vpImage &I, vpImage &dIx, const double *filter, unsigned int size); - static inline double filterX(const vpImage &I, - unsigned int r, unsigned int c, - const double *filter, unsigned int size) + static inline double filterX(const vpImage &I, unsigned int r, unsigned int c, const double *filter, + unsigned int size) { double result; @@ -219,10 +193,8 @@ class VISP_EXPORT vpImageFilter return result + filter[0] * I[r][c]; } - static inline double filterXLeftBorder(const vpImage &I, - unsigned int r, unsigned int c, - const double *filter, - unsigned int size) + static inline double filterXLeftBorder(const vpImage &I, unsigned int r, unsigned int c, + const double *filter, unsigned int size) { double result; @@ -237,10 +209,8 @@ class VISP_EXPORT vpImageFilter return result + filter[0] * I[r][c]; } - static inline double filterXRightBorder(const vpImage &I, - unsigned int r, unsigned int c, - const double *filter, - unsigned int size) + static inline double filterXRightBorder(const vpImage &I, unsigned int r, unsigned int c, + const double *filter, unsigned int size) { double result; @@ -250,14 +220,12 @@ class VISP_EXPORT vpImageFilter 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]); + 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 &I, unsigned int r, - unsigned int c, const double *filter, + static inline double filterX(const vpImage &I, unsigned int r, unsigned int c, const double *filter, unsigned int size) { double result; @@ -270,9 +238,7 @@ class VISP_EXPORT vpImageFilter return result + filter[0] * I[r][c]; } - static inline double filterXLeftBorder(const vpImage &I, - unsigned int r, unsigned int c, - const double *filter, + static inline double filterXLeftBorder(const vpImage &I, unsigned int r, unsigned int c, const double *filter, unsigned int size) { double result; @@ -288,10 +254,8 @@ class VISP_EXPORT vpImageFilter return result + filter[0] * I[r][c]; } - static inline double filterXRightBorder(const vpImage &I, - unsigned int r, unsigned int c, - const double *filter, - unsigned int size) + static inline double filterXRightBorder(const vpImage &I, unsigned int r, unsigned int c, + const double *filter, unsigned int size) { double result; @@ -301,19 +265,15 @@ class VISP_EXPORT vpImageFilter 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]); + 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 &I, vpImage &dIx, - const double *filter, unsigned int size); - static void filterY(const vpImage &I, vpImage &dIx, - const double *filter, unsigned int size); - static inline double filterY(const vpImage &I, - unsigned int r, unsigned int c, - const double *filter, unsigned int size) + static void filterY(const vpImage &I, vpImage &dIx, const double *filter, unsigned int size); + static void filterY(const vpImage &I, vpImage &dIx, const double *filter, unsigned int size); + static inline double filterY(const vpImage &I, unsigned int r, unsigned int c, const double *filter, + unsigned int size) { double result; @@ -325,10 +285,8 @@ class VISP_EXPORT vpImageFilter return result + filter[0] * I[r][c]; } - double static inline filterYTopBorder(const vpImage &I, - unsigned int r, unsigned int c, - const double *filter, - unsigned int size) + double static inline filterYTopBorder(const vpImage &I, unsigned int r, unsigned int c, + const double *filter, unsigned int size) { double result; @@ -343,10 +301,8 @@ class VISP_EXPORT vpImageFilter return result + filter[0] * I[r][c]; } - double static inline filterYBottomBorder(const vpImage &I, - unsigned int r, unsigned int c, - const double *filter, - unsigned int size) + double static inline filterYBottomBorder(const vpImage &I, unsigned int r, unsigned int c, + const double *filter, unsigned int size) { double result; @@ -356,15 +312,12 @@ class VISP_EXPORT vpImageFilter 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]); + 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 &I, - unsigned int r, unsigned int c, - const double *filter, + static inline double filterYTopBorder(const vpImage &I, unsigned int r, unsigned int c, const double *filter, unsigned int size) { double result; @@ -380,10 +333,8 @@ class VISP_EXPORT vpImageFilter return result + filter[0] * I[r][c]; } - static inline double filterYBottomBorder(const vpImage &I, - unsigned int r, unsigned int c, - const double *filter, - unsigned int size) + static inline double filterYBottomBorder(const vpImage &I, unsigned int r, unsigned int c, + const double *filter, unsigned int size) { double result; @@ -393,14 +344,12 @@ class VISP_EXPORT vpImageFilter 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]); + 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 &I, unsigned int r, - unsigned int c, const double *filter, + static inline double filterY(const vpImage &I, unsigned int r, unsigned int c, const double *filter, unsigned int size) { double result; @@ -413,11 +362,9 @@ class VISP_EXPORT vpImageFilter return result + filter[0] * I[r][c]; } - static void gaussianBlur(const vpImage &I, - vpImage &GI, unsigned int size = 7, + static void gaussianBlur(const vpImage &I, vpImage &GI, unsigned int size = 7, double sigma = 0., bool normalize = true); - static void gaussianBlur(const vpImage &I, vpImage &GI, - unsigned int size = 7, double sigma = 0., + static void gaussianBlur(const vpImage &I, vpImage &GI, unsigned int size = 7, double sigma = 0., bool normalize = true); /*! Apply a 5x5 Gaussian filter to an image pixel. @@ -426,63 +373,39 @@ class VISP_EXPORT vpImageFilter \param r : coordinates (row) of the pixel \param c : coordinates (column) of the pixel */ - template - static double gaussianFilter(const vpImage &fr, const unsigned int r, - const unsigned int c) + template static double gaussianFilter(const vpImage &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])) / + 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 &I, - vpImage &GI); - static void getGaussXPyramidal(const vpImage &I, - vpImage &GI); - static void getGaussYPyramidal(const vpImage &I, - vpImage &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); + static void getGaussPyramidal(const vpImage &I, vpImage &GI); + static void getGaussXPyramidal(const vpImage &I, vpImage &GI); + static void getGaussYPyramidal(const vpImage &I, vpImage &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 &I, vpImage &dIx); - static void getGradX(const vpImage &I, vpImage &dIx, - const double *filter, unsigned int size); - static void getGradX(const vpImage &I, vpImage &dIx, - const double *filter, unsigned int size); - static void getGradXGauss2D(const vpImage &I, - vpImage &dIx, - const double *gaussianKernel, - const double *gaussianDerivativeKernel, - unsigned int size); + static void getGradX(const vpImage &I, vpImage &dIx, const double *filter, unsigned int size); + static void getGradX(const vpImage &I, vpImage &dIx, const double *filter, unsigned int size); + static void getGradXGauss2D(const vpImage &I, vpImage &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 &I, vpImage &dIy); - static void getGradY(const vpImage &I, vpImage &dIy, - const double *filter, unsigned int size); - static void getGradY(const vpImage &I, vpImage &dIy, - const double *filter, unsigned int size); - static void getGradYGauss2D(const vpImage &I, - vpImage &dIy, - const double *gaussianKernel, - const double *gaussianDerivativeKernel, - unsigned int size); + static void getGradY(const vpImage &I, vpImage &dIy, const double *filter, unsigned int size); + static void getGradY(const vpImage &I, vpImage &dIy, const double *filter, unsigned int size); + static void getGradYGauss2D(const vpImage &I, vpImage &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 index bb5d913c6a..e8fe26231a 100644 --- a/modules/core/include/visp3/core/vpImageMorphology.h +++ b/modules/core/include/visp3/core/vpImageMorphology.h @@ -80,17 +80,13 @@ class VISP_EXPORT vpImageMorphology public: template - static void erosion(vpImage &I, Type value, Type value_out, - vpConnexityType connexity = CONNEXITY_4); + static void erosion(vpImage &I, Type value, Type value_out, vpConnexityType connexity = CONNEXITY_4); template - static void dilatation(vpImage &I, Type value, Type value_out, - vpConnexityType connexity = CONNEXITY_4); + static void dilatation(vpImage &I, Type value, Type value_out, vpConnexityType connexity = CONNEXITY_4); - static void erosion(vpImage &I, - const vpConnexityType &connexity = CONNEXITY_4); - static void dilatation(vpImage &I, - const vpConnexityType &connexity = CONNEXITY_4); + static void erosion(vpImage &I, const vpConnexityType &connexity = CONNEXITY_4); + static void dilatation(vpImage &I, const vpConnexityType &connexity = CONNEXITY_4); }; /*! @@ -111,8 +107,7 @@ class VISP_EXPORT vpImageMorphology \sa dilatation() */ template -void vpImageMorphology::erosion(vpImage &I, Type value, Type value_out, - vpConnexityType connexity) +void vpImageMorphology::erosion(vpImage &I, Type value, Type value_out, vpConnexityType connexity) { if (I.getSize() == 0) { std::cerr << "Input image is empty!" << std::endl; @@ -152,11 +147,9 @@ void vpImageMorphology::erosion(vpImage &I, Type value, Type value_out, for (unsigned int j = 0; j < I.getWidth(); j++) { if (J[i + 1][j + 1] == value) { // Consider 8 neighbors - if ((J[i][j] == value_out) || (J[i][j + 1] == value_out) || - (J[i][j + 2] == value_out) || (J[i + 1][j] == value_out) || - (J[i + 1][j + 2] == value_out) || (J[i + 2][j] == value_out) || - (J[i + 2][j + 1] == value_out) || - (J[i + 2][j + 2] == value_out)) + if ((J[i][j] == value_out) || (J[i][j + 1] == value_out) || (J[i][j + 2] == value_out) || + (J[i + 1][j] == value_out) || (J[i + 1][j + 2] == value_out) || (J[i + 2][j] == value_out) || + (J[i + 2][j + 1] == value_out) || (J[i + 2][j + 2] == value_out)) I[i][j] = value_out; } } @@ -182,8 +175,7 @@ void vpImageMorphology::erosion(vpImage &I, Type value, Type value_out, \sa erosion() */ template -void vpImageMorphology::dilatation(vpImage &I, Type value, - Type value_out, vpConnexityType connexity) +void vpImageMorphology::dilatation(vpImage &I, Type value, Type value_out, vpConnexityType connexity) { if (I.getSize() == 0) { std::cerr << "Input image is empty!" << std::endl; @@ -223,10 +215,9 @@ void vpImageMorphology::dilatation(vpImage &I, Type value, for (unsigned int j = 0; j < I.getWidth(); j++) { if (J[i + 1][j + 1] == value_out) { // Consider 8 neighbors - if ((J[i][j] == value) || (J[i][j + 1] == value) || - (J[i][j + 2] == value) || (J[i + 1][j] == value) || - (J[i + 1][j + 2] == value) || (J[i + 2][j] == value) || - (J[i + 2][j + 1] == value) || (J[i + 2][j + 2] == value)) { + if ((J[i][j] == value) || (J[i][j + 1] == value) || (J[i][j + 2] == value) || (J[i + 1][j] == value) || + (J[i + 1][j + 2] == value) || (J[i + 2][j] == value) || (J[i + 2][j + 1] == value) || + (J[i + 2][j + 2] == value)) { I[i][j] = value; } } diff --git a/modules/core/include/visp3/core/vpImagePoint.h b/modules/core/include/visp3/core/vpImagePoint.h index 9211ba9b31..91a6c2e636 100644 --- a/modules/core/include/visp3/core/vpImagePoint.h +++ b/modules/core/include/visp3/core/vpImagePoint.h @@ -284,8 +284,7 @@ class VISP_EXPORT vpImagePoint */ 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()))); + return (sqrt(vpMath::sqr(iP1.get_i() - iP2.get_i()) + vpMath::sqr(iP1.get_j() - iP2.get_j()))); } static vpRect getBBox(const std::vector &ipVec); @@ -301,40 +300,25 @@ class VISP_EXPORT vpImagePoint */ 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())); + 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 unsigned 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 unsigned 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); + 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 unsigned 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 unsigned 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; diff --git a/modules/core/include/visp3/core/vpImageTools.h b/modules/core/include/visp3/core/vpImageTools.h index 8a3c25dbd9..25420c1995 100644 --- a/modules/core/include/visp3/core/vpImageTools.h +++ b/modules/core/include/visp3/core/vpImageTools.h @@ -82,77 +82,53 @@ class VISP_EXPORT vpImageTools }; template - static inline void binarise(vpImage &I, Type threshold1, - Type threshold2, Type value1, Type value2, - Type value3, const bool useLUT = true); - static void changeLUT(vpImage &I, unsigned char A, - unsigned char newA, unsigned char B, + static inline void binarise(vpImage &I, Type threshold1, Type threshold2, Type value1, Type value2, Type value3, + const bool useLUT = true); + static void changeLUT(vpImage &I, unsigned char A, unsigned char newA, unsigned char B, unsigned char newB); template - static void crop(const vpImage &I, double roi_top, double roi_left, - unsigned int roi_height, unsigned int roi_width, - vpImage &crop, unsigned int v_scale = 1, - unsigned int h_scale = 1); + static void crop(const vpImage &I, double roi_top, double roi_left, unsigned int roi_height, + unsigned int roi_width, vpImage &crop, unsigned int v_scale = 1, unsigned int h_scale = 1); template - static void crop(const vpImage &I, const vpImagePoint &topLeft, - unsigned int roi_height, unsigned int roi_width, - vpImage &crop, unsigned int v_scale = 1, - unsigned int h_scale = 1); + static void crop(const vpImage &I, const vpImagePoint &topLeft, unsigned int roi_height, unsigned int roi_width, + vpImage &crop, unsigned int v_scale = 1, unsigned int h_scale = 1); template - static void crop(const vpImage &I, const vpRect &roi, - vpImage &crop, unsigned int v_scale = 1, + static void crop(const vpImage &I, const vpRect &roi, vpImage &crop, unsigned int v_scale = 1, unsigned int h_scale = 1); template - static void crop(const unsigned char *bitmap, unsigned int width, - unsigned int height, const vpRect &roi, - vpImage &crop, unsigned int v_scale = 1, - unsigned int h_scale = 1); + static void crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi, + vpImage &crop, unsigned int v_scale = 1, unsigned int h_scale = 1); - template - static void flip(const vpImage &I, vpImage &newI); + template static void flip(const vpImage &I, vpImage &newI); template static void flip(vpImage &I); - static void imageDifference(const vpImage &I1, - const vpImage &I2, + static void imageDifference(const vpImage &I1, const vpImage &I2, vpImage &Idiff); - static void imageDifference(const vpImage &I1, - const vpImage &I2, - vpImage &Idiff); + static void imageDifference(const vpImage &I1, const vpImage &I2, vpImage &Idiff); - static void imageDifferenceAbsolute(const vpImage &I1, - const vpImage &I2, + static void imageDifferenceAbsolute(const vpImage &I1, const vpImage &I2, vpImage &Idiff); - static void imageDifferenceAbsolute(const vpImage &I1, - const vpImage &I2, - vpImage &Idiff); + static void imageDifferenceAbsolute(const vpImage &I1, const vpImage &I2, vpImage &Idiff); - static void imageAdd(const vpImage &I1, - const vpImage &I2, - vpImage &Ires, + static void imageAdd(const vpImage &I1, const vpImage &I2, vpImage &Ires, const bool saturate = false); - static void imageSubtract(const vpImage &I1, - const vpImage &I2, - vpImage &Ires, - const bool saturate = false); + static void imageSubtract(const vpImage &I1, const vpImage &I2, + vpImage &Ires, const bool saturate = false); template - static void - resize(const vpImage &I, vpImage &Ires, - const unsigned int width, const unsigned int height, - const vpImageInterpolationType &method = INTERPOLATION_NEAREST); + static void resize(const vpImage &I, vpImage &Ires, const unsigned int width, const unsigned int height, + const vpImageInterpolationType &method = INTERPOLATION_NEAREST); template - static void - resize(const vpImage &I, vpImage &Ires, - const vpImageInterpolationType &method = INTERPOLATION_NEAREST); + static void resize(const vpImage &I, vpImage &Ires, + const vpImageInterpolationType &method = INTERPOLATION_NEAREST); template - static void undistort(const vpImage &I, const vpCameraParameters &cam, - vpImage &newI); + static void undistort(const vpImage &I, const vpCameraParameters &cam, vpImage &newI); #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) /*! @@ -160,45 +136,33 @@ class VISP_EXPORT vpImageTools */ //@{ template - vp_deprecated static void - createSubImage(const vpImage &I, unsigned int i_sub, - unsigned int j_sub, unsigned int nrow_sub, - unsigned int ncol_sub, vpImage &S); + vp_deprecated static void createSubImage(const vpImage &I, unsigned int i_sub, unsigned int j_sub, + unsigned int nrow_sub, unsigned int ncol_sub, vpImage &S); template - vp_deprecated static void createSubImage(const vpImage &I, - const vpRect &rect, - vpImage &S); + vp_deprecated static void createSubImage(const vpImage &I, const vpRect &rect, vpImage &S); //@} #endif private: // Cubic interpolation - static float cubicHermite(const float A, const float B, const float C, - const float D, const float t); + static float cubicHermite(const float A, const float B, const float C, const float D, const float t); - template - static Type getPixelClamped(const vpImage &I, const float u, - const float v); + template static Type getPixelClamped(const vpImage &I, const float u, const float v); // Linear interpolation static float lerp(const float A, const float B, const float t); template - static void resizeBicubic(const vpImage &I, vpImage &Ires, - const unsigned int i, const unsigned int j, - const float u, const float v, const float xFrac, - const float yFrac); + static void resizeBicubic(const vpImage &I, vpImage &Ires, const unsigned int i, const unsigned int j, + const float u, const float v, const float xFrac, const float yFrac); template - static void resizeBilinear(const vpImage &I, vpImage &Ires, - const unsigned int i, const unsigned int j, - const float u, const float v, const float xFrac, - const float yFrac); + static void resizeBilinear(const vpImage &I, vpImage &Ires, const unsigned int i, const unsigned int j, + const float u, const float v, const float xFrac, const float yFrac); template - static void resizeNearest(const vpImage &I, vpImage &Ires, - const unsigned int i, const unsigned int j, + static void resizeNearest(const vpImage &I, vpImage &Ires, const unsigned int i, const unsigned int j, const float u, const float v); }; @@ -222,10 +186,8 @@ class VISP_EXPORT vpImageTools */ template -void vpImageTools::createSubImage(const vpImage &I, - unsigned int roi_top, unsigned int roi_left, - unsigned int roi_height, - unsigned int roi_width, vpImage &crop) +void vpImageTools::createSubImage(const vpImage &I, unsigned int roi_top, unsigned int roi_left, + unsigned int roi_height, unsigned int roi_width, vpImage &crop) { vpImageTools::crop(I, roi_top, roi_left, roi_height, roi_width, crop); } @@ -245,9 +207,7 @@ void vpImageTools::createSubImage(const vpImage &I, \sa crop(const vpImage &, const vpRect &, vpImage &) */ -template -void vpImageTools::createSubImage(const vpImage &I, const vpRect &roi, - vpImage &crop) +template void vpImageTools::createSubImage(const vpImage &I, const vpRect &roi, vpImage &crop) { vpImageTools::crop(I, roi, crop); } @@ -274,17 +234,13 @@ void vpImageTools::createSubImage(const vpImage &I, const vpRect &roi, */ template -void vpImageTools::crop(const vpImage &I, double roi_top, - double roi_left, unsigned int roi_height, - unsigned int roi_width, vpImage &crop, - unsigned int v_scale, unsigned int h_scale) +void vpImageTools::crop(const vpImage &I, double roi_top, double roi_left, unsigned int roi_height, + unsigned int roi_width, vpImage &crop, unsigned int v_scale, unsigned int h_scale) { int i_min = (std::max)((int)(ceil(roi_top / v_scale)), 0); int j_min = (std::max)((int)(ceil(roi_left / h_scale)), 0); - int i_max = (std::min)((int)(ceil((roi_top + roi_height)) / v_scale), - (int)(I.getHeight() / v_scale)); - int j_max = (std::min)((int)(ceil((roi_left + roi_width) / h_scale)), - (int)(I.getWidth() / h_scale)); + int i_max = (std::min)((int)(ceil((roi_top + roi_height)) / v_scale), (int)(I.getHeight() / v_scale)); + int j_max = (std::min)((int)(ceil((roi_left + roi_width) / h_scale)), (int)(I.getWidth() / h_scale)); unsigned int i_min_u = (unsigned int)i_min; unsigned int j_min_u = (unsigned int)j_min; @@ -334,13 +290,10 @@ void vpImageTools::crop(const vpImage &I, double roi_top, */ template -void vpImageTools::crop(const vpImage &I, const vpImagePoint &topLeft, - unsigned int roi_height, unsigned int roi_width, - vpImage &crop, unsigned int v_scale, - unsigned int h_scale) +void vpImageTools::crop(const vpImage &I, const vpImagePoint &topLeft, unsigned int roi_height, + unsigned int roi_width, vpImage &crop, unsigned int v_scale, unsigned int h_scale) { - vpImageTools::crop(I, topLeft.get_i(), topLeft.get_j(), roi_height, - roi_width, crop, v_scale, h_scale); + vpImageTools::crop(I, topLeft.get_i(), topLeft.get_j(), roi_height, roi_width, crop, v_scale, h_scale); } /*! @@ -361,13 +314,11 @@ void vpImageTools::crop(const vpImage &I, const vpImagePoint &topLeft, */ template -void vpImageTools::crop(const vpImage &I, const vpRect &roi, - vpImage &crop, unsigned int v_scale, +void vpImageTools::crop(const vpImage &I, const vpRect &roi, vpImage &crop, unsigned int v_scale, unsigned int h_scale) { - vpImageTools::crop(I, roi.getTop(), roi.getLeft(), - (unsigned int)roi.getHeight(), - (unsigned int)roi.getWidth(), crop, v_scale, h_scale); + vpImageTools::crop(I, roi.getTop(), roi.getLeft(), (unsigned int)roi.getHeight(), (unsigned int)roi.getWidth(), crop, + v_scale, h_scale); } /*! @@ -388,19 +339,13 @@ void vpImageTools::crop(const vpImage &I, const vpRect &roi, \param h_scale [in] : Horizontal subsampling factor applied to the ROI. */ template -void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, - unsigned int height, const vpRect &roi, - vpImage &crop, unsigned int v_scale, - unsigned int h_scale) +void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi, + vpImage &crop, unsigned int v_scale, unsigned int h_scale) { int i_min = (std::max)((int)(ceil(roi.getTop() / v_scale)), 0); int j_min = (std::max)((int)(ceil(roi.getLeft() / h_scale)), 0); - int i_max = - (std::min)((int)(ceil((roi.getTop() + roi.getHeight())) / v_scale), - (int)(height / v_scale)); - int j_max = - (std::min)((int)(ceil((roi.getLeft() + roi.getWidth()) / h_scale)), - (int)(width / h_scale)); + int i_max = (std::min)((int)(ceil((roi.getTop() + roi.getHeight())) / v_scale), (int)(height / v_scale)); + int j_max = (std::min)((int)(ceil((roi.getLeft() + roi.getWidth()) / h_scale)), (int)(width / h_scale)); unsigned int i_min_u = (unsigned int)i_min; unsigned int j_min_u = (unsigned int)j_min; @@ -412,23 +357,19 @@ void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, if (v_scale == 1 && h_scale == 1) { for (unsigned int i = 0; i < r_height; i++) { - void *src = - (void *)(bitmap + ((i + i_min_u) * width + j_min_u) * sizeof(Type)); + void *src = (void *)(bitmap + ((i + i_min_u) * width + j_min_u) * sizeof(Type)); void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } } else if (h_scale == 1) { for (unsigned int i = 0; i < r_height; i++) { - void *src = - (void *)(bitmap + ((i + i_min_u) * width * v_scale + j_min_u) * - sizeof(Type)); + void *src = (void *)(bitmap + ((i + i_min_u) * width * v_scale + j_min_u) * sizeof(Type)); void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } } else { for (unsigned int i = 0; i < r_height; i++) { - unsigned int i_src = - (i + i_min_u) * width * v_scale + j_min_u * h_scale; + unsigned int i_src = (i + i_min_u) * width * v_scale + j_min_u * h_scale; for (unsigned int j = 0; j < r_width; j++) { void *src = (void *)(bitmap + (i_src + j * h_scale) * sizeof(Type)); void *dst = (void *)&crop[i][j]; @@ -451,14 +392,11 @@ void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, */ template -inline void vpImageTools::binarise(vpImage &I, Type threshold1, - Type threshold2, Type value1, Type value2, +inline void vpImageTools::binarise(vpImage &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; + std::cerr << "LUT not available for this type ! Will use the iteration method." << std::endl; } Type v; @@ -488,11 +426,8 @@ inline void vpImageTools::binarise(vpImage &I, Type threshold1, */ template <> -inline void vpImageTools::binarise(vpImage &I, - unsigned char threshold1, - unsigned char threshold2, - unsigned char value1, unsigned char value2, - unsigned char value3, const bool useLUT) +inline void vpImageTools::binarise(vpImage &I, unsigned char threshold1, unsigned char threshold2, + unsigned char value1, unsigned char value2, unsigned char value3, const bool useLUT) { if (useLUT) { // Construct the LUT @@ -532,14 +467,9 @@ template class vpUndistortInternalType unsigned int threadid; public: - vpUndistortInternalType() - : src(NULL), dst(NULL), width(0), height(0), cam(), nthreads(0), - threadid(0){}; + vpUndistortInternalType() : src(NULL), dst(NULL), width(0), height(0), cam(), nthreads(0), threadid(0){}; - vpUndistortInternalType(const vpUndistortInternalType &u) - { - *this = u; - }; + vpUndistortInternalType(const vpUndistortInternalType &u) { *this = u; }; vpUndistortInternalType &operator=(const vpUndistortInternalType &u) { src = u.src; @@ -556,11 +486,9 @@ template class vpUndistortInternalType static void *vpUndistort_threaded(void *arg); }; -template -void *vpUndistortInternalType::vpUndistort_threaded(void *arg) +template void *vpUndistortInternalType::vpUndistort_threaded(void *arg) { - vpUndistortInternalType *undistortSharedData = - static_cast *>(arg); + vpUndistortInternalType *undistortSharedData = static_cast *>(arg); int offset = (int)undistortSharedData->threadid; int width = (int)undistortSharedData->width; int height = (int)undistortSharedData->height; @@ -581,8 +509,7 @@ void *vpUndistortInternalType::vpUndistort_threaded(void *arg) Type *dst = undistortSharedData->dst + (height / nthreads * offset) * width; Type *src = undistortSharedData->src; - for (double v = height / nthreads * offset; - v < height / nthreads * (offset + 1); v++) { + 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; @@ -609,8 +536,7 @@ void *vpUndistortInternalType::vpUndistort_threaded(void *arg) 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))) { + 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)); @@ -650,9 +576,7 @@ void *vpUndistortInternalType::vpUndistort_threaded(void *arg) or "Charon"(Intel Xeon 3 GHz, 2Go RAM) : ~8 ms for a 640x480 image. */ template -void vpImageTools::undistort(const vpImage &I, - const vpCameraParameters &cam, - vpImage &undistI) +void vpImageTools::undistort(const vpImage &I, const vpCameraParameters &cam, vpImage &undistI) { #ifdef VISP_HAVE_PTHREAD // @@ -691,9 +615,7 @@ void vpImageTools::undistort(const vpImage &I, undistortSharedData[i].cam = cam; undistortSharedData[i].nthreads = nthreads; undistortSharedData[i].threadid = i; - pthread_create(&callThd[i], &attr, - &vpUndistortInternalType::vpUndistort_threaded, - &undistortSharedData[i]); + pthread_create(&callThd[i], &attr, &vpUndistortInternalType::vpUndistort_threaded, &undistortSharedData[i]); } pthread_attr_destroy(&attr); /* Wait on the other threads */ @@ -763,8 +685,7 @@ void vpImageTools::undistort(const vpImage &I, 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))) { + 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)); @@ -818,8 +739,7 @@ void vpImageTools::undistort(const vpImage &I, \param newI : Output image which is the flipped input image. */ -template -void vpImageTools::flip(const vpImage &I, vpImage &newI) +template void vpImageTools::flip(const vpImage &I, vpImage &newI) { unsigned int height = 0, width = 0; @@ -828,8 +748,7 @@ void vpImageTools::flip(const vpImage &I, vpImage &newI) 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)); + memcpy(newI.bitmap + i * width, I.bitmap + (height - 1 - i) * width, width * sizeof(Type)); } } @@ -859,8 +778,8 @@ int main() // Flip the image vpImageTools::flip(I); - vpImageIo::write(I, "Klimt-flip.ppm"); // Write the image in a PGM P5 image -file format + // Write the image in a PGM P5 image file format + vpImageIo::write(I, "Klimt-flip.ppm"); } \endcode */ @@ -877,16 +796,12 @@ template void vpImageTools::flip(vpImage &I) 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)); + 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)); } } -template -Type vpImageTools::getPixelClamped(const vpImage &I, const float u, - const float v) +template Type vpImageTools::getPixelClamped(const vpImage &I, const float u, const float v) { unsigned int i, j; if (u < 0.) @@ -909,10 +824,9 @@ Type vpImageTools::getPixelClamped(const vpImage &I, const float u, // Reference: // http://blog.demofox.org/2015/08/15/resizing-images-with-bicubic-interpolation/ template -void vpImageTools::resizeBicubic(const vpImage &I, vpImage &Ires, - const unsigned int i, const unsigned int j, - const float u, const float v, - const float xFrac, const float yFrac) +void vpImageTools::resizeBicubic(const vpImage &I, vpImage &Ires, const unsigned int i, + const unsigned int j, const float u, const float v, const float xFrac, + const float yFrac) { // 1st row Type p00 = getPixelClamped(I, u - 1, v - 1); @@ -947,11 +861,9 @@ void vpImageTools::resizeBicubic(const vpImage &I, vpImage &Ires, } template <> -inline void -vpImageTools::resizeBicubic(const vpImage &I, vpImage &Ires, - const unsigned int i, const unsigned int j, - const float u, const float v, const float xFrac, - const float yFrac) +inline void vpImageTools::resizeBicubic(const vpImage &I, vpImage &Ires, const unsigned int i, + const unsigned int j, const float u, const float v, const float xFrac, + const float yFrac) { // 1st row vpRGBa p00 = getPixelClamped(I, u - 1, v - 1); @@ -978,30 +890,24 @@ vpImageTools::resizeBicubic(const vpImage &I, vpImage &Ires, vpRGBa p33 = getPixelClamped(I, u + 2, v + 2); for (int c = 0; c < 3; c++) { - float col0 = cubicHermite( - ((unsigned char *)&p00)[c], ((unsigned char *)&p01)[c], - ((unsigned char *)&p02)[c], ((unsigned char *)&p03)[c], xFrac); - float col1 = cubicHermite( - ((unsigned char *)&p10)[c], ((unsigned char *)&p11)[c], - ((unsigned char *)&p12)[c], ((unsigned char *)&p13)[c], xFrac); - float col2 = cubicHermite( - ((unsigned char *)&p20)[c], ((unsigned char *)&p21)[c], - ((unsigned char *)&p22)[c], ((unsigned char *)&p23)[c], xFrac); - float col3 = cubicHermite( - ((unsigned char *)&p30)[c], ((unsigned char *)&p31)[c], - ((unsigned char *)&p32)[c], ((unsigned char *)&p33)[c], xFrac); + float col0 = cubicHermite(((unsigned char *)&p00)[c], ((unsigned char *)&p01)[c], ((unsigned char *)&p02)[c], + ((unsigned char *)&p03)[c], xFrac); + float col1 = cubicHermite(((unsigned char *)&p10)[c], ((unsigned char *)&p11)[c], ((unsigned char *)&p12)[c], + ((unsigned char *)&p13)[c], xFrac); + float col2 = cubicHermite(((unsigned char *)&p20)[c], ((unsigned char *)&p21)[c], ((unsigned char *)&p22)[c], + ((unsigned char *)&p23)[c], xFrac); + float col3 = cubicHermite(((unsigned char *)&p30)[c], ((unsigned char *)&p31)[c], ((unsigned char *)&p32)[c], + ((unsigned char *)&p33)[c], xFrac); float value = cubicHermite(col0, col1, col2, col3, yFrac); - ((unsigned char *)&Ires[i][j])[c] = - vpMath::saturate(value); + ((unsigned char *)&Ires[i][j])[c] = vpMath::saturate(value); } } template -void vpImageTools::resizeBilinear(const vpImage &I, vpImage &Ires, - const unsigned int i, const unsigned int j, - const float u, const float v, - const float xFrac, const float yFrac) +void vpImageTools::resizeBilinear(const vpImage &I, vpImage &Ires, const unsigned int i, + const unsigned int j, const float u, const float v, const float xFrac, + const float yFrac) { unsigned int u0 = (unsigned int)u; unsigned int v0 = (unsigned int)v; @@ -1023,11 +929,9 @@ void vpImageTools::resizeBilinear(const vpImage &I, vpImage &Ires, } template <> -inline void -vpImageTools::resizeBilinear(const vpImage &I, vpImage &Ires, - const unsigned int i, const unsigned int j, - const float u, const float v, const float xFrac, - const float yFrac) +inline void vpImageTools::resizeBilinear(const vpImage &I, vpImage &Ires, const unsigned int i, + const unsigned int j, const float u, const float v, const float xFrac, + const float yFrac) { unsigned int u0 = (unsigned int)u; unsigned int v0 = (unsigned int)v; @@ -1042,21 +946,17 @@ vpImageTools::resizeBilinear(const vpImage &I, vpImage &Ires, unsigned int v3 = (std::min)(I.getHeight() - 1, (unsigned int)v + 1); for (int c = 0; c < 3; c++) { - float col0 = lerp(((unsigned char *)&I[v0][u0])[c], - ((unsigned char *)&I[v1][u1])[c], xFrac); - float col1 = lerp(((unsigned char *)&I[v2][u2])[c], - ((unsigned char *)&I[v3][u3])[c], xFrac); + float col0 = lerp(((unsigned char *)&I[v0][u0])[c], ((unsigned char *)&I[v1][u1])[c], xFrac); + float col1 = lerp(((unsigned char *)&I[v2][u2])[c], ((unsigned char *)&I[v3][u3])[c], xFrac); float value = lerp(col0, col1, yFrac); - ((unsigned char *)&Ires[i][j])[c] = - vpMath::saturate(value); + ((unsigned char *)&Ires[i][j])[c] = vpMath::saturate(value); } } template -void vpImageTools::resizeNearest(const vpImage &I, vpImage &Ires, - const unsigned int i, const unsigned int j, - const float u, const float v) +void vpImageTools::resizeNearest(const vpImage &I, vpImage &Ires, const unsigned int i, + const unsigned int j, const float u, const float v) { Ires[i][j] = getPixelClamped(I, u, v); } @@ -1072,9 +972,8 @@ void vpImageTools::resizeNearest(const vpImage &I, vpImage &Ires, \param method : Interpolation method. */ template -void vpImageTools::resize(const vpImage &I, vpImage &Ires, - const unsigned int width, const unsigned int height, - const vpImageInterpolationType &method) +void vpImageTools::resize(const vpImage &I, vpImage &Ires, const unsigned int width, + const unsigned int height, const vpImageInterpolationType &method) { Ires.resize(height, width); @@ -1090,11 +989,9 @@ void vpImageTools::resize(const vpImage &I, vpImage &Ires, the desired size). \param method : Interpolation method. */ template -void vpImageTools::resize(const vpImage &I, vpImage &Ires, - const vpImageInterpolationType &method) +void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpImageInterpolationType &method) { - if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || - Ires.getHeight() < 2) { + if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || Ires.getHeight() < 2) { std::cerr << "Input or output image is too small!" << std::endl; return; } diff --git a/modules/core/include/visp3/core/vpIoException.h b/modules/core/include/visp3/core/vpIoException.h index dbc1da672c..db24d3268d 100644 --- a/modules/core/include/visp3/core/vpIoException.h +++ b/modules/core/include/visp3/core/vpIoException.h @@ -92,10 +92,7 @@ class VISP_EXPORT vpIoException : public vpException setMessage(format, args); va_end(args); } - vpIoException(const int id, const std::string &msg) : vpException(id, msg) - { - ; - } + vpIoException(const int id, const std::string &msg) : vpException(id, msg) { ; } explicit vpIoException(const int id) : vpException(id) { ; } }; diff --git a/modules/core/include/visp3/core/vpIoTools.h b/modules/core/include/visp3/core/vpIoTools.h index 29367a8e7e..836470b25e 100644 --- a/modules/core/include/visp3/core/vpIoTools.h +++ b/modules/core/include/visp3/core/vpIoTools.h @@ -63,9 +63,9 @@ name and then create a file in this directory. \code -#include #include #include +#include #include int main() @@ -87,9 +87,8 @@ int main() // 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" - + // Under Windows converts the filename string into "username\\file.txt" + filename = vpIoTools::path(filename); std::cout << "Create: " << filename << std::endl; f.open(filename.c_str()); f.close(); @@ -107,9 +106,13 @@ string into "username\\file.txt" } \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 + 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 @@ -123,29 +126,27 @@ 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 + 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 - + // full name <- "/home/user/data/exp2" + vpIoTools::setBaseName("exp" + nExp); + // full name <- "/home/user/data/exp2" since use2D==false + vpIoTools::addNameElement("2D", use2D); + // full name <- "/home/user/data/exp2_3D" + vpIoTools::addNameElement("3D", use3D); + // full name <- "/home/user/data/exp2_3D_lambda0.4" + vpIoTools::addNameElement("lambda", lambda); + + // Saving file.Would copy "/home/user/demo/config.txt" to + // "/home/user/data/exp2_3D_lambda0.4_config.txt" if doSave was true + vpIoTools::saveConfigFile(doSave); // create sub directory - vpIoTools::createBaseNamePath(); // creates -"/home/user/data/exp2_3D_lambda0.4/" + vpIoTools::createBaseNamePath(); // creates "/home/user/data/exp2_3D_lambda0.4/" } \endcode @@ -161,8 +162,7 @@ class VISP_EXPORT vpIoTools 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 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); @@ -174,8 +174,7 @@ class VISP_EXPORT vpIoTools 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 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); @@ -192,20 +191,15 @@ class VISP_EXPORT vpIoTools #endif static std::string getAbsolutePathname(const std::string &pathname); - static std::string getFileExtension(const std::string &pathname, - const bool checkFile = false); + 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 std::string createFilePath(const std::string &parent, const std::string &child); static bool isAbsolutePathname(const std::string &pathname); - static bool isSamePathname(const std::string &pathname1, - const std::string &pathname2); - static std::pair - splitDrive(const std::string &pathname); - static std::vector splitChain(const std::string &chain, - const std::string &sep); + static bool isSamePathname(const std::string &pathname1, const std::string &pathname2); + static std::pair splitDrive(const std::string &pathname); + static std::vector splitChain(const std::string &chain, const std::string &sep); static std::vector getDirFiles(const std::string &dirname); /*! @@ -221,16 +215,13 @@ class VISP_EXPORT vpIoTools 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 &value, - const unsigned int &nCols = 0, + static bool readConfigVar(const std::string &var, vpArray2D &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 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(); diff --git a/modules/core/include/visp3/core/vpKalmanFilter.h b/modules/core/include/visp3/core/vpKalmanFilter.h index 32fdb78eee..64926e0dcd 100644 --- a/modules/core/include/visp3/core/vpKalmanFilter.h +++ b/modules/core/include/visp3/core/vpKalmanFilter.h @@ -129,8 +129,7 @@ class VISP_EXPORT vpKalmanFilter public: vpKalmanFilter(); explicit vpKalmanFilter(unsigned int n_signal); - vpKalmanFilter(unsigned int size_state, unsigned int size_measure, - unsigned int n_signal); + vpKalmanFilter(unsigned int size_state, unsigned int size_measure, unsigned int n_signal); /*! Destructor that does noting. */ virtual ~vpKalmanFilter(){}; /*! @@ -139,8 +138,7 @@ class VISP_EXPORT vpKalmanFilter 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 init(unsigned int size_state, unsigned int size_measure, unsigned int n_signal); void prediction(); void filtering(const vpColVector &z); /*! diff --git a/modules/core/include/visp3/core/vpLine.h b/modules/core/include/visp3/core/vpLine.h index 7afae00f84..5f1acda5dc 100644 --- a/modules/core/include/visp3/core/vpLine.h +++ b/modules/core/include/visp3/core/vpLine.h @@ -156,10 +156,8 @@ class VISP_EXPORT vpLine : public vpForwardProjection */ 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 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); @@ -170,13 +168,10 @@ class VISP_EXPORT vpLine : public vpForwardProjection void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP); void changeFrame(const vpHomogeneousMatrix &cMo); - void display(const vpImage &I, const vpCameraParameters &cam, - const vpColor &color = vpColor::green, - const unsigned int thickness = 1); - void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &color = vpColor::green, + void display(const vpImage &I, const vpCameraParameters &cam, const vpColor &color = vpColor::green, const unsigned int thickness = 1); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &color = vpColor::green, const unsigned int thickness = 1); vpLine *duplicate() const; }; diff --git a/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h b/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h index a09ba740c7..8d67e01113 100644 --- a/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h +++ b/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h @@ -101,25 +101,21 @@ class VISP_EXPORT vpLinearKalmanFilterInstantiation : public vpKalmanFilter //@{ inline void setStateModel(vpStateModel model); - void initFilter(unsigned int nsignal, vpColVector &sigma_state, - vpColVector &sigma_measure, double rho, double dt); + 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); + 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); + void initStateConstAccWithColoredNoise_MeasureVel(unsigned int nsignal, vpColVector &sigma_state, + vpColVector &sigma_measure, double rho, double dt); //@} protected: @@ -141,9 +137,10 @@ 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 + // Value is 2 + std::cout << "State vector size: " << kalman.getStateSize() << std::endl; + // Value is 1 + std::cout << "Measure vector size: " << kalman.getMeasureSize() << std::endl; } \endcode */ diff --git a/modules/core/include/visp3/core/vpList.h b/modules/core/include/visp3/core/vpList.h index 8ed997a2b5..e62055f0c4 100644 --- a/modules/core/include/visp3/core/vpList.h +++ b/modules/core/include/visp3/core/vpList.h @@ -146,11 +146,10 @@ template class vpList 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 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 @@ -166,18 +165,13 @@ template class vpList 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 + 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 &operator=(const vpList &l); inline void operator+=(vpList &l); @@ -222,11 +216,7 @@ template void vpList::init() \endverbatim \sa init() */ -template -vpList::vpList() : nb(0), first(NULL), last(NULL), cur(NULL) -{ - init(); -} +template vpList::vpList() : nb(0), first(NULL), last(NULL), cur(NULL) { init(); } /*! \brief vpList destructor @@ -243,18 +233,12 @@ template vpList::~vpList() /*! \brief return the number of element in the list */ -template unsigned int vpList::nbElement(void) -{ - return (nb); -} +template unsigned int vpList::nbElement(void) { return (nb); } /*! \brief return the number of element in the list */ -template unsigned int vpList::nbElements(void) -{ - return (nb); -} +template unsigned int vpList::nbElements(void) { return (nb); } /*! \brief position the current element on the next one @@ -292,10 +276,7 @@ template type &vpList::value(void) { return (cur->val); } ^ \endverbatim */ -template const type &vpList::value(void) const -{ - return (cur->val); -} +template const type &vpList::value(void) const { return (cur->val); } /*! \brief return the value of the previous element @@ -305,10 +286,7 @@ template const type &vpList::value(void) const ^ \endverbatim */ -template type &vpList::previousValue(void) -{ - return (cur->prev->val); -} +template type &vpList::previousValue(void) { return (cur->prev->val); } /*! \brief return the value of the next element @@ -317,10 +295,7 @@ template type &vpList::previousValue(void) ^ \endverbatim */ -template type &vpList::nextValue(void) -{ - return (cur->next->val); -} +template type &vpList::nextValue(void) { return (cur->next->val); } /*! \brief return the first element of the list @@ -328,20 +303,14 @@ template type &vpList::nextValue(void) [*, a, b, c, d, *] --> firstValue() return a \endverbatim */ -template type &vpList::firstValue(void) -{ - return (first->next->val); -} +template type &vpList::firstValue(void) { return (first->next->val); } /*!\brief return the last element of the list \verbatim [*, a, b, c, d, *] --> lastValue() return d \endverbatim */ -template type &vpList::lastValue(void) -{ - return (last->prev->val); -} +template type &vpList::lastValue(void) { return (last->prev->val); } /*! \brief Position the current element on the first element of the list @@ -371,10 +340,7 @@ template void vpList::end(void) { cur = last->prev; } [*, *] --> empty return true \endverbatim */ -template bool vpList::empty(void) const -{ - return ((first->next == last) && (first == last->prev)); -} +template bool vpList::empty(void) const { return ((first->next == last) && (first == last->prev)); } /*! \brief Test if the current element is outside the list @@ -387,11 +353,7 @@ template bool vpList::empty(void) const ^ or ^ \endverbatim */ -template bool vpList::outside(void) const -{ - - return ((cur == first) || (cur == last)); -} +template bool vpList::outside(void) const { return ((cur == first) || (cur == last)); } /*! \brief Test if the next element is outside the list @@ -547,10 +509,7 @@ template void vpList::addLeft(type &v) ^ ^ \endverbatim */ -template void vpList::modify(const type &v) -{ - cur->val = v; -} +template void vpList::modify(const type &v) { cur->val = v; } /*! \brief Switch the current element with the element on the left @@ -578,8 +537,7 @@ template void vpList::swapLeft() prevTmp->prev = cur; prevTmp->next = nextTmp; } else { - std::cout << "vpList: previous element is outside (swapLeft) " - << std::endl; + std::cout << "vpList: previous element is outside (swapLeft) " << std::endl; } } @@ -661,8 +619,7 @@ template void vpList::suppress(void) \param l : the list to copy */ -template -vpList &vpList::operator=(const vpList &l) +template vpList &vpList::operator=(const vpList &l) { type x; vpListElement *e; @@ -722,9 +679,7 @@ template void vpList::operator+=(const type &l) \param l : the list to copy */ -template -vpList::vpList(const vpList &l) - : nb(0), first(NULL), last(NULL), cur(NULL) +template vpList::vpList(const vpList &l) : nb(0), first(NULL), last(NULL), cur(NULL) { init(); *this = l; diff --git a/modules/core/include/visp3/core/vpMath.h b/modules/core/include/visp3/core/vpMath.h index df3c9e73c5..99c81be05c 100644 --- a/modules/core/include/visp3/core/vpMath.h +++ b/modules/core/include/visp3/core/vpMath.h @@ -53,9 +53,8 @@ #include #include -#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) +#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 #endif @@ -135,10 +134,7 @@ class VISP_EXPORT vpMath \param b : Second number. \return The maximum of the two numbers. */ - template static Type maximum(const Type &a, const Type &b) - { - return (a > b) ? a : b; - } + template static Type maximum(const Type &a, const Type &b) { return (a > b) ? a : b; } /*! Find the minimum between two numbers (or other). @@ -146,20 +142,14 @@ class VISP_EXPORT vpMath \param b : Second number. \return The minimum of the two numbers. */ - template static Type minimum(const Type &a, const Type &b) - { - return (a < b) ? a : b; - } + template 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 static Type abs(const Type &x) - { - return (x < 0) ? -x : x; - } + template static Type abs(const Type &x) { return (x < 0) ? -x : x; } // sinus cardinal static double sinc(double x); @@ -168,8 +158,7 @@ class VISP_EXPORT vpMath static double msinc(double sinx, double x); // sigmoid - static inline double sigmoid(double x, double x0 = 0., double x1 = 1., - double n = 12.); + static inline double sigmoid(double x, double x0 = 0., double x1 = 1., double n = 12.); /*! Exchange two numbers. @@ -187,40 +176,18 @@ class VISP_EXPORT vpMath static bool isNaN(const double value); static bool isInf(const double value); - template static inline _Tp saturate(unsigned char v) - { - return _Tp(v); - } - template static inline _Tp saturate(char v) - { - return _Tp(v); - } - template static inline _Tp saturate(unsigned short v) - { - return _Tp(v); - } - template static inline _Tp saturate(short v) - { - return _Tp(v); - } - template static inline _Tp saturate(unsigned v) - { - return _Tp(v); - } + template static inline _Tp saturate(unsigned char v) { return _Tp(v); } + template static inline _Tp saturate(char v) { return _Tp(v); } + template static inline _Tp saturate(unsigned short v) { return _Tp(v); } + template static inline _Tp saturate(short v) { return _Tp(v); } + template static inline _Tp saturate(unsigned v) { return _Tp(v); } template static inline _Tp saturate(int v) { return _Tp(v); } - template static inline _Tp saturate(float v) - { - return _Tp(v); - } - template static inline _Tp saturate(double v) - { - return _Tp(v); - } + template static inline _Tp saturate(float v) { return _Tp(v); } + template static inline _Tp saturate(double v) { return _Tp(v); } static double getMean(const std::vector &v); static double getMedian(const std::vector &v); - static double getStdev(const std::vector &v, - const bool useBesselCorrection = false); + static double getStdev(const std::vector &v, const bool useBesselCorrection = false); static int modulo(const int a, const int n); @@ -269,7 +236,7 @@ int vpMath::round(const double x) { #if defined(VISP_HAVE_FUNC_ROUND) //:: to design the global namespace and avoid to call recursively - //vpMath::round + // vpMath::round return (int)::round(x); #elif defined(VISP_HAVE_FUNC_STD_ROUND) return (int)std::round(x); @@ -348,8 +315,7 @@ double vpMath::sigmoid(double x, double x0, double x1, double n) 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); + return (1. / (1. + exp(-n * ((x - x0) / (x1 - x0) - 0.5))) - l0) / (l1 - l0); } // unsigned char @@ -366,27 +332,19 @@ template <> inline unsigned char vpMath::saturate(char v) return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v); } -template <> -inline unsigned char vpMath::saturate(unsigned short v) +template <> inline unsigned char vpMath::saturate(unsigned short v) { - return (unsigned char)((std::min))((unsigned int)v, - (unsigned int)UCHAR_MAX); + return (unsigned char)((std::min))((unsigned int)v, (unsigned int)UCHAR_MAX); } template <> inline unsigned char vpMath::saturate(int v) { - return (unsigned char)((unsigned int)v <= UCHAR_MAX - ? v - : v > 0 ? UCHAR_MAX : 0); + return (unsigned char)((unsigned int)v <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0); } -template <> inline unsigned char vpMath::saturate(short v) -{ - return saturate((int)v); -} +template <> inline unsigned char vpMath::saturate(short v) { return saturate((int)v); } -template <> -inline unsigned char vpMath::saturate(unsigned int v) +template <> inline unsigned char vpMath::saturate(unsigned int v) { return (unsigned char)((std::min))(v, (unsigned int)UCHAR_MAX); } @@ -404,10 +362,7 @@ template <> inline unsigned char vpMath::saturate(double v) } // char -template <> inline char vpMath::saturate(unsigned char v) -{ - return (char)((std::min))((int)v, SCHAR_MAX); -} +template <> inline char vpMath::saturate(unsigned char v) { return (char)((std::min))((int)v, SCHAR_MAX); } template <> inline char vpMath::saturate(unsigned short v) { @@ -416,15 +371,10 @@ template <> inline char vpMath::saturate(unsigned short v) template <> inline char vpMath::saturate(int v) { - return (char)((unsigned int)(v - SCHAR_MIN) <= (unsigned int)UCHAR_MAX - ? v - : v > 0 ? SCHAR_MAX : SCHAR_MIN); + return (char)((unsigned int)(v - SCHAR_MIN) <= (unsigned int)UCHAR_MAX ? v : v > 0 ? SCHAR_MAX : SCHAR_MIN); } -template <> inline char vpMath::saturate(short v) -{ - return saturate((int)v); -} +template <> inline char vpMath::saturate(short v) { return saturate((int)v); } template <> inline char vpMath::saturate(unsigned int v) { @@ -464,13 +414,10 @@ template <> inline unsigned short vpMath::saturate(short v) template <> inline unsigned short vpMath::saturate(int v) { - return (unsigned short)((unsigned int)v <= (unsigned int)USHRT_MAX - ? v - : v > 0 ? USHRT_MAX : 0); + return (unsigned short)((unsigned int)v <= (unsigned int)USHRT_MAX ? v : v > 0 ? USHRT_MAX : 0); } -template <> -inline unsigned short vpMath::saturate(unsigned int v) +template <> inline unsigned short vpMath::saturate(unsigned int v) { return (unsigned short)((std::min))(v, (unsigned int)USHRT_MAX); } @@ -488,15 +435,10 @@ template <> inline unsigned short vpMath::saturate(double v) } // short -template <> inline short vpMath::saturate(unsigned short v) -{ - return (short)((std::min))((int)v, SHRT_MAX); -} +template <> inline short vpMath::saturate(unsigned short v) { return (short)((std::min))((int)v, SHRT_MAX); } template <> inline short vpMath::saturate(int v) { - return (short)((unsigned int)(v - SHRT_MIN) <= (unsigned int)USHRT_MAX - ? v - : v > 0 ? SHRT_MAX : SHRT_MIN); + return (short)((unsigned int)(v - SHRT_MIN) <= (unsigned int)USHRT_MAX ? v : v > 0 ? SHRT_MAX : SHRT_MIN); } template <> inline short vpMath::saturate(unsigned int v) { @@ -514,27 +456,15 @@ template <> inline short vpMath::saturate(double v) } // int -template <> inline int vpMath::saturate(float v) -{ - return vpMath::round(v); -} +template <> inline int vpMath::saturate(float v) { return vpMath::round(v); } -template <> inline int vpMath::saturate(double v) -{ - return vpMath::round(v); -} +template <> inline int vpMath::saturate(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(float v) -{ - return (unsigned int)vpMath::round(v); -} +template <> inline unsigned int vpMath::saturate(float v) { return (unsigned int)vpMath::round(v); } -template <> inline unsigned int vpMath::saturate(double v) -{ - return (unsigned int)vpMath::round(v); -} +template <> inline unsigned int vpMath::saturate(double v) { return (unsigned int)vpMath::round(v); } #endif diff --git a/modules/core/include/visp3/core/vpMatrix.h b/modules/core/include/visp3/core/vpMatrix.h index 5219e7d669..0b001389c5 100644 --- a/modules/core/include/visp3/core/vpMatrix.h +++ b/modules/core/include/visp3/core/vpMatrix.h @@ -132,19 +132,19 @@ class VISP_EXPORT vpMatrix : public vpArray2D \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(r, c, val) - { - } - vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, - unsigned int nrows, unsigned int ncols); + vpMatrix(unsigned int r, unsigned int c, double val) : vpArray2D(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 + matrix: +\code +vpRotationMatrix R; +vpMatrix M(R); +\endcode */ vpMatrix(const vpArray2D &A) : vpArray2D(A) {} @@ -239,16 +239,12 @@ class VISP_EXPORT vpMatrix : public vpArray2D //------------------------------------------------- /** @name Columns, rows, sub-matrices extraction */ //@{ - vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, - unsigned int ncols) const; + vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const; vpColVector getCol(const unsigned int j) const; - vpColVector getCol(const unsigned int j, const unsigned int i_begin, - const unsigned int size) const; + vpColVector getCol(const unsigned int j, const unsigned int i_begin, const unsigned int size) const; vpRowVector getRow(const unsigned int i) const; - vpRowVector getRow(const unsigned int i, const unsigned int j_begin, - const unsigned int size) const; - void init(const vpMatrix &M, unsigned int r, unsigned int c, - unsigned int nrows, unsigned int ncols); + vpRowVector getRow(const unsigned int i, const unsigned int j_begin, const unsigned int size) const; + void init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols); //@} //--------------------------------- @@ -399,55 +395,39 @@ class VISP_EXPORT vpMatrix : public vpArray2D vpMatrix pseudoInverse(double svThreshold = 1e-6) const; unsigned int pseudoInverse(vpMatrix &Ap, double svThreshold = 1e-6) const; - unsigned int pseudoInverse(vpMatrix &Ap, vpColVector &sv, - double svThreshold = 1e-6) const; - unsigned int pseudoInverse(vpMatrix &Ap, vpColVector &sv, - double svThreshold, vpMatrix &imA, - vpMatrix &imAt) const; - unsigned int pseudoInverse(vpMatrix &Ap, vpColVector &sv, - double svThreshold, vpMatrix &imA, - vpMatrix &imAt, vpMatrix &kerAt) const; + unsigned int pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold = 1e-6) const; + unsigned int pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt) const; + unsigned int pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt, + vpMatrix &kerAt) const; #ifndef DOXYGEN_SHOULD_SKIP_THIS #if defined(VISP_HAVE_LAPACK) vpMatrix pseudoInverseLapack(double svThreshold = 1e-6) const; - unsigned int pseudoInverseLapack(vpMatrix &Ap, - double svThreshold = 1e-6) const; - unsigned int pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, - double svThreshold = 1e-6) const; - unsigned int pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, - double svThreshold, vpMatrix &imA, - vpMatrix &imAt, vpMatrix &kerAt) const; + unsigned int pseudoInverseLapack(vpMatrix &Ap, double svThreshold = 1e-6) const; + unsigned int pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold = 1e-6) const; + unsigned int pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt, + vpMatrix &kerAt) const; #endif #if defined(VISP_HAVE_EIGEN3) vpMatrix pseudoInverseEigen3(double svThreshold = 1e-6) const; - unsigned int pseudoInverseEigen3(vpMatrix &Ap, - double svThreshold = 1e-6) const; - unsigned int pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, - double svThreshold = 1e-6) const; - unsigned int pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, - double svThreshold, vpMatrix &imA, - vpMatrix &imAt, vpMatrix &kerAt) const; + unsigned int pseudoInverseEigen3(vpMatrix &Ap, double svThreshold = 1e-6) const; + unsigned int pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold = 1e-6) const; + unsigned int pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt, + vpMatrix &kerAt) const; #endif #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) vpMatrix pseudoInverseOpenCV(double svThreshold = 1e-6) const; - unsigned int pseudoInverseOpenCV(vpMatrix &Ap, - double svThreshold = 1e-6) const; - unsigned int pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, - double svThreshold = 1e-6) const; - unsigned int pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, - double svThreshold, vpMatrix &imA, - vpMatrix &imAt, vpMatrix &kerAt) const; + unsigned int pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold = 1e-6) const; + unsigned int pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold = 1e-6) const; + unsigned int pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt, + vpMatrix &kerAt) const; #endif #if defined(VISP_HAVE_GSL) vpMatrix pseudoInverseGsl(double svThreshold = 1e-6) const; - unsigned int pseudoInverseGsl(vpMatrix &Ap, - double svThreshold = 1e-6) const; - unsigned int pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, - double svThreshold = 1e-6) const; - unsigned int pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, - double svThreshold, vpMatrix &imA, - vpMatrix &imAt, vpMatrix &kerAt) const; + unsigned int pseudoInverseGsl(vpMatrix &Ap, double svThreshold = 1e-6) const; + unsigned int pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold = 1e-6) const; + unsigned int pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt, + vpMatrix &kerAt) const; #endif #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -510,18 +490,12 @@ class VISP_EXPORT vpMatrix : public vpArray2D //--------------------------------- /** @name Printing */ //@{ - std::ostream &cppPrint(std::ostream &os, - const std::string &matrixName = "A", - bool octet = false) const; + std::ostream &cppPrint(std::ostream &os, const std::string &matrixName = "A", bool octet = false) const; std::ostream &csvPrint(std::ostream &os) const; std::ostream &maplePrint(std::ostream &os) const; std::ostream &matlabPrint(std::ostream &os) const; - int print(std::ostream &s, unsigned int length, - char const *intro = 0) const; - void printSize() const - { - std::cout << getRows() << " x " << getCols() << " "; - } + int print(std::ostream &s, unsigned int length, char const *intro = 0) const; + void printSize() const { std::cout << getRows() << " x " << getCols() << " "; } //@} //------------------------------------------------------------------ @@ -543,12 +517,10 @@ class VISP_EXPORT vpMatrix : public vpArray2D /** @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); + 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); + 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 @@ -558,8 +530,7 @@ class VISP_EXPORT vpMatrix : public vpArray2D // 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); + 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); @@ -575,27 +546,18 @@ class VISP_EXPORT vpMatrix : public vpArray2D /** @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, + 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 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); + static void sub2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C); //@} //--------------------------------- @@ -615,19 +577,12 @@ class VISP_EXPORT vpMatrix : public vpArray2D //--------------------------------- /** @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, + 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, + 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); //@} @@ -647,9 +602,7 @@ class VISP_EXPORT vpMatrix : public vpArray2D \return Returns true if no problem appends. */ - static inline bool loadMatrix(const std::string &filename, - vpArray2D &M, - const bool binary = false, + static inline bool loadMatrix(const std::string &filename, vpArray2D &M, const bool binary = false, char *header = NULL) { return vpArray2D::load(filename, M, binary, header); @@ -665,8 +618,7 @@ class VISP_EXPORT vpMatrix : public vpArray2D \return Returns true if no problem appends. */ - static inline bool loadMatrixYAML(const std::string &filename, - vpArray2D &M, char *header = NULL) + static inline bool loadMatrixYAML(const std::string &filename, vpArray2D &M, char *header = NULL) { return vpArray2D::loadYAML(filename, M, header); } @@ -685,9 +637,7 @@ class VISP_EXPORT vpMatrix : public vpArray2D 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 &M, - const bool binary = false, + static inline bool saveMatrix(const std::string &filename, const vpArray2D &M, const bool binary = false, const char *header = "") { return vpArray2D::save(filename, M, binary, header); @@ -705,9 +655,7 @@ class VISP_EXPORT vpMatrix : public vpArray2D \return Returns true if success. */ - static inline bool saveMatrixYAML(const std::string &filename, - const vpArray2D &M, - const char *header = "") + static inline bool saveMatrixYAML(const std::string &filename, const vpArray2D &M, const char *header = "") { return vpArray2D::saveYAML(filename, M, header); } @@ -732,44 +680,32 @@ class VISP_EXPORT vpMatrix : public vpArray2D \deprecated You should rather use stack(const vpMatrix &A, const vpMatrix &B) */ - vp_deprecated static vpMatrix stackMatrices(const vpMatrix &A, - const vpMatrix &B) - { - return stack(A, B); - } + vp_deprecated static vpMatrix stackMatrices(const vpMatrix &A, const vpMatrix &B) { return 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) - { - stack(A, B, C); - } + vp_deprecated static void stackMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) { 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); + 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); + 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); + 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); + vp_deprecated static void stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C); /*! \deprecated You should rather use diag(const double &) @@ -784,20 +720,15 @@ class VISP_EXPORT vpMatrix : public vpArray2D private: #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) - static void blas_dgemm(char trans_a, char trans_b, const int M, const int N, - const int K, double alpha, double *a_data, - const int lda, double *b_data, const int ldb, - double beta, double *c_data, const int ldc); - static void blas_dgemv(char trans, const int M, const int N, double alpha, - double *a_data, const int lda, double *x_data, - const int incx, double beta, double *y_data, - const int incy); + static void blas_dgemm(char trans_a, char trans_b, const int M, const int N, const int K, double alpha, + double *a_data, const int lda, double *b_data, const int ldb, double beta, double *c_data, + const int ldc); + static void blas_dgemv(char trans, const int M, const int N, double alpha, double *a_data, const int lda, + double *x_data, const int incx, double beta, double *y_data, const int incy); #endif - static void computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, - const vpColVector &deltaS, - const vpMatrix &Ls, vpMatrix &Js, - vpColVector &deltaP); + static void computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, + vpMatrix &Js, vpColVector &deltaP); }; ////////////////////////////////////////////////////////////////////////// diff --git a/modules/core/include/visp3/core/vpMatrixException.h b/modules/core/include/visp3/core/vpMatrixException.h index 3d9cf5b1f5..325f4b5975 100644 --- a/modules/core/include/visp3/core/vpMatrixException.h +++ b/modules/core/include/visp3/core/vpMatrixException.h @@ -101,11 +101,7 @@ class VISP_EXPORT vpMatrixException : public vpException setMessage(format, args); va_end(args); } - vpMatrixException(const int id, const std::string &msg) - : vpException(id, msg) - { - ; - } + vpMatrixException(const int id, const std::string &msg) : vpException(id, msg) { ; } explicit vpMatrixException(const int id) : vpException(id) { ; } // vpMatrixException() : vpException() { ;} }; diff --git a/modules/core/include/visp3/core/vpMeterPixelConversion.h b/modules/core/include/visp3/core/vpMeterPixelConversion.h index 584cf8fc5d..c443843f8d 100644 --- a/modules/core/include/visp3/core/vpMeterPixelConversion.h +++ b/modules/core/include/visp3/core/vpMeterPixelConversion.h @@ -66,12 +66,10 @@ class VISP_EXPORT vpMeterPixelConversion { public: - static void convertEllipse(const vpCameraParameters &cam, - const vpCircle &circle, vpImagePoint ¢er, + 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, + static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p); /*! @@ -97,9 +95,7 @@ class VISP_EXPORT vpMeterPixelConversion distortion. */ - inline static void convertPoint(const vpCameraParameters &cam, - const double &x, const double &y, double &u, - double &v) + inline static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v) { switch (cam.projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: @@ -135,9 +131,7 @@ class VISP_EXPORT vpMeterPixelConversion distortion. */ - inline static void convertPoint(const vpCameraParameters &cam, - const double &x, const double &y, - vpImagePoint &iP) + inline static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, vpImagePoint &iP) { switch (cam.projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: @@ -159,10 +153,8 @@ class VISP_EXPORT vpMeterPixelConversion \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) + 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; @@ -178,10 +170,8 @@ class VISP_EXPORT vpMeterPixelConversion \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) + 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); @@ -203,10 +193,8 @@ class VISP_EXPORT vpMeterPixelConversion \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) + 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; @@ -229,9 +217,7 @@ class VISP_EXPORT vpMeterPixelConversion \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, + inline static void convertPointWithDistortion(const vpCameraParameters &cam, const double &x, const double &y, vpImagePoint &iP) { double r2 = 1. + cam.kud * (x * x + y * y); diff --git a/modules/core/include/visp3/core/vpMoment.h b/modules/core/include/visp3/core/vpMoment.h index a8c43c56ba..b46256a959 100644 --- a/modules/core/include/visp3/core/vpMoment.h +++ b/modules/core/include/visp3/core/vpMoment.h @@ -158,7 +158,6 @@ class VISP_EXPORT vpMoment virtual void printDependencies(std::ostream &os) const; void update(vpMomentObject &object); //@} - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMoment &m); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMoment &m); }; #endif diff --git a/modules/core/include/visp3/core/vpMomentAlpha.h b/modules/core/include/visp3/core/vpMomentAlpha.h index 6efb93f860..c9a746fa0d 100644 --- a/modules/core/include/visp3/core/vpMomentAlpha.h +++ b/modules/core/include/visp3/core/vpMomentAlpha.h @@ -81,15 +81,15 @@ performing a 180 degrees rotation. Therefore the first and second alpha should have opposite values. \code -#include -#include -#include -#include -#include -#include -#include #include #include +#include +#include +#include +#include +#include +#include +#include //generic function for printing void print (double i) { std::cout << i << "\t";} @@ -97,24 +97,29 @@ void print (double i) { std::cout << i << "\t";} int main() { vpPoint p; - std::vector 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 + // vector that contains the vertices of the contour polygon + std::vector vec_p; + + // coordinates in meters in the image plane (vertex 1) + p.set_x(1); p.set_y(1); + vec_p.push_back(p); + // coordinates in meters in the image plane (vertex 2) + p.set_x(2); p.set_y(2); + vec_p.push_back(p); + // coordinates in meters in the image plane (vertex 3) + p.set_x(-3); p.set_y(0); + vec_p.push_back(p); + // coordinates in meters in the image plane (vertex 4) + p.set_x(-3); p.set_y(-1); + 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 + // object is the inner part of a polygon + objRef.setType(vpMomentObject::DENSE_POLYGON); + objRef.fromVector(vec_p); // Init the dense object with the polygon vpMomentDatabase dbRef; //reference database vpMomentGravityCenter gRef; // declaration of gravity center @@ -125,8 +130,7 @@ polygon 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 + 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 @@ -142,34 +146,34 @@ alpha 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; + 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); - + 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 + 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 + 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 @@ -186,7 +190,6 @@ reference return 0; } - \endcode This program outputs: \code @@ -241,8 +244,7 @@ class VISP_EXPORT vpMomentAlpha : public vpMoment return false; } - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentAlpha &v); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentAlpha &v); void printDependencies(std::ostream &os) const; }; diff --git a/modules/core/include/visp3/core/vpMomentArea.h b/modules/core/include/visp3/core/vpMomentArea.h index ac297bd69e..dcb81803ba 100644 --- a/modules/core/include/visp3/core/vpMomentArea.h +++ b/modules/core/include/visp3/core/vpMomentArea.h @@ -64,8 +64,7 @@ class VISP_EXPORT vpMomentArea : public vpMoment const char *name() const { return "vpMomentArea"; } void printDependencies(std::ostream &os) const; //@} - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentArea &m); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentArea &m); }; #endif diff --git a/modules/core/include/visp3/core/vpMomentAreaNormalized.h b/modules/core/include/visp3/core/vpMomentAreaNormalized.h index b4aaee5080..3029cd300c 100644 --- a/modules/core/include/visp3/core/vpMomentAreaNormalized.h +++ b/modules/core/include/visp3/core/vpMomentAreaNormalized.h @@ -81,25 +81,25 @@ continous cases. This example shows a computation in the discrete case. \code -#include -#include -#include -#include #include #include +#include +#include +#include +#include int main() { vpPoint p; std::vector 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); + 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 + //////////////////////////////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 @@ -158,8 +158,7 @@ class VISP_EXPORT vpMomentAreaNormalized : public vpMoment Moment name. */ const char *name() const { return "vpMomentAreaNormalized"; } - friend VISP_EXPORT std::ostream & - operator<<(std::ostream &os, const vpMomentAreaNormalized &v); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentAreaNormalized &v); void printDependencies(std::ostream &os) const; }; diff --git a/modules/core/include/visp3/core/vpMomentBasic.h b/modules/core/include/visp3/core/vpMomentBasic.h index b49d212951..7041c82ee0 100644 --- a/modules/core/include/visp3/core/vpMomentBasic.h +++ b/modules/core/include/visp3/core/vpMomentBasic.h @@ -84,8 +84,7 @@ class VISP_EXPORT vpMomentBasic : public vpMoment Moment name. */ const char *name() const { return "vpMomentBasic"; } - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentBasic &v); + 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 index 3d875b466b..6d32820365 100644 --- a/modules/core/include/visp3/core/vpMomentCInvariant.h +++ b/modules/core/include/visp3/core/vpMomentCInvariant.h @@ -78,38 +78,41 @@ get shortcuts. The example below shows how to retrieve the \f$C_2\f$ invariant: \code -#include -#include -#include #include #include +#include +#include +#include int main() { vpPoint p; std::vector 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 + 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.); + 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(db.get("vpMomentCInvariant",success)); if(success) + const vpMomentCInvariant& C + = static_cast(db.get("vpMomentCInvariant",success)); + if(success) std::cout << C.get(1) << std:: endl; // print C2 invariant else std::cout << "vpMomentCInvariant not found." << std::endl; @@ -129,18 +132,15 @@ class VISP_EXPORT vpMomentCInvariant : public vpMoment std::vector c; std::vector s; double K; - void computeI(const vpMomentCentered &momentCentered, - std::vector &I); + void computeI(const vpMomentCentered &momentCentered, std::vector &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 - cn; // same as s above but calculated from normalized moments - std::vector - sn; // same as c above but calculated from normalized moments - double In1; // same as I1 in Sx,Sy formulae but calculated from normalized - // moments + std::vector cn; // same as s above but calculated from normalized moments + std::vector 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: @@ -282,7 +282,6 @@ class VISP_EXPORT vpMomentCInvariant : public vpMoment */ inline const std::vector &getMomentVector() const { return values; } - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentCInvariant &v); + 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 index b52aa892de..ea76e0d41b 100644 --- a/modules/core/include/visp3/core/vpMomentCentered.h +++ b/modules/core/include/visp3/core/vpMomentCentered.h @@ -91,8 +91,7 @@ class VISP_EXPORT vpMomentCentered : public vpMoment */ inline const char *name() const { return "vpMomentCentered"; } - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentCentered &v); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentCentered &v); void printWithIndices(std::ostream &os) const; void printDependencies(std::ostream &os) const; @@ -108,15 +107,24 @@ where order is the object's order. 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 + 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; + this kind of code: + + \code +vpMomentCentered mc; //[...] mc.compute(); double mu12; @@ -124,9 +132,6 @@ 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 &vpMomentCentered::get() const -{ - return vpMoment::get(); -} +inline const std::vector &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 index 15c9f06c78..e983262a6c 100644 --- a/modules/core/include/visp3/core/vpMomentCommon.h +++ b/modules/core/include/visp3/core/vpMomentCommon.h @@ -131,8 +131,7 @@ class VISP_EXPORT vpMomentCommon : public vpMomentDatabase //#endif public: - vpMomentCommon(double dstSurface, const std::vector &ref, - double refAlpha, double dstZ = 1.0, + vpMomentCommon(double dstSurface, const std::vector &ref, double refAlpha, double dstZ = 1.0, bool flg_sxsyfromnormalized = false); virtual ~vpMomentCommon(); diff --git a/modules/core/include/visp3/core/vpMomentDatabase.h b/modules/core/include/visp3/core/vpMomentDatabase.h index b83101cda8..4ac5e7df9a 100644 --- a/modules/core/include/visp3/core/vpMomentDatabase.h +++ b/modules/core/include/visp3/core/vpMomentDatabase.h @@ -67,25 +67,29 @@ 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 - #include #include - #include +of a dependency between two moments using a vpMomentDatabase: + +\code #include #include +#include +#include +#include +#include int main() { vpPoint p; - std::vector 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) + std::vector 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 + // two discrete points obj.fromVector(vec_p); // Init the dense object with the polygon vpMomentDatabase db; @@ -94,7 +98,7 @@ image moment object with 1 as g.linkTo(db); //add gravity center to database mc.linkTo(db); //centered moments depend on gravity, add them to the - //database to grant access + //database to grant access db.updateAll(obj); // All of the moments must be updated, not just mc @@ -103,9 +107,8 @@ image moment object with 1 as 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 + std::cout << "Gravity center: " << g << std:: endl; // print gravity center moment + std::cout << "Centered moments: " << mc << std:: endl; // print centered moment return 0; } @@ -130,10 +133,7 @@ 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; - } + bool operator()(char const *a, char const *b) const { return std::strcmp(a, b) < 0; } }; #endif std::map moments; @@ -157,8 +157,7 @@ class VISP_EXPORT vpMomentDatabase //@} friend class vpMoment; - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentDatabase &v); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentDatabase &v); }; #endif diff --git a/modules/core/include/visp3/core/vpMomentGravityCenter.h b/modules/core/include/visp3/core/vpMomentGravityCenter.h index aaf4287b92..55fe47be3e 100644 --- a/modules/core/include/visp3/core/vpMomentGravityCenter.h +++ b/modules/core/include/visp3/core/vpMomentGravityCenter.h @@ -55,36 +55,39 @@ class vpMomentObject; \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$. + 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 #include - #include + \f$,\f$y_g = \frac{m_{10}}{m_{00}} \f$ +\code +#include #include +#include +#include int main() { // Define the contour of an object by a 5 clockwise vertices on a plane vpPoint p; - std::vector vec_p; // vector that contains the vertices of the -contour polygon + std::vector 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) + 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); - 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 + // 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 @@ -133,8 +136,7 @@ class VISP_EXPORT vpMomentGravityCenter : public vpMoment const char *name() const { return "vpMomentGravityCenter"; } void printDependencies(std::ostream &os) const; //@} - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentGravityCenter &v); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentGravityCenter &v); }; #endif diff --git a/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h b/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h index b73bb39870..9359684f35 100644 --- a/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h +++ b/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h @@ -62,8 +62,7 @@ class vpMomentObject; 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 +class VISP_EXPORT vpMomentGravityCenterNormalized : public vpMomentGravityCenter { public: vpMomentGravityCenterNormalized(); @@ -72,8 +71,7 @@ class VISP_EXPORT vpMomentGravityCenterNormalized //! Moment name. const char *name() const { return "vpMomentGravityCenterNormalized"; } void printDependencies(std::ostream &os) const; - friend VISP_EXPORT std::ostream & - operator<<(std::ostream &os, const vpMomentGravityCenterNormalized &v); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentGravityCenterNormalized &v); }; #endif diff --git a/modules/core/include/visp3/core/vpMomentObject.h b/modules/core/include/visp3/core/vpMomentObject.h index 51b41f1812..9e1f1edf4b 100644 --- a/modules/core/include/visp3/core/vpMomentObject.h +++ b/modules/core/include/visp3/core/vpMomentObject.h @@ -209,8 +209,8 @@ 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 +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 @@ -250,16 +250,15 @@ class VISP_EXPORT vpMomentObject Virtual destructor to allow polymorphic usage. For instance, \code - vpMomentObject* obj = new vpWeightedMomentObject(weightfunc,ORDER); where - vpWeightedMomentObject is child class of vpMomentObject \endcode + vpMomentObject* obj = new vpWeightedMomentObject(weightfunc,ORDER); + \endcode + where vpWeightedMomentObject is child class of vpMomentObject */ virtual ~vpMomentObject(); void fromImage(const vpImage &image, unsigned char threshold, const vpCameraParameters &cam); // Binary version - void fromImage(const vpImage &image, - const vpCameraParameters &cam, - vpCameraImgBckGrndType bg_type, + void fromImage(const vpImage &image, const vpCameraParameters &cam, vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size = true); // Photometric version void fromVector(std::vector &points); @@ -281,15 +280,13 @@ class VISP_EXPORT vpMomentObject void init(unsigned int orderinp); void init(const vpMomentObject &objin); - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentObject &v); + 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); + static void printWithIndices(const vpMomentObject &momobj, std::ostream &os); /*! Specifies the type of the input data. \param input_type : An input type. @@ -310,10 +307,8 @@ class VISP_EXPORT vpMomentObject void cacheValues(std::vector &cache, double x, double y); private: - void cacheValues(std::vector &cache, double x, double y, - double IntensityNormalized); - double calc_mom_polygon(unsigned int p, unsigned int q, - const std::vector &points); + void cacheValues(std::vector &cache, double x, double y, double IntensityNormalized); + double calc_mom_polygon(unsigned int p, unsigned int q, const std::vector &points); }; #endif diff --git a/modules/core/include/visp3/core/vpMutex.h b/modules/core/include/visp3/core/vpMutex.h index e9d7d6ede8..c0820346ce 100644 --- a/modules/core/include/visp3/core/vpMutex.h +++ b/modules/core/include/visp3/core/vpMutex.h @@ -82,9 +82,9 @@ class vpMutex #ifdef WINRT_8_1 m_mutex = CreateMutexEx(NULL, NULL, 0, NULL); #else - m_mutex = CreateMutex(NULL, // default security attributes - FALSE, // initially not owned - NULL); // unnamed mutex + m_mutex = CreateMutex(NULL, // default security attributes + FALSE, // initially not owned + NULL); // unnamed mutex #endif if (m_mutex == NULL) { std::cout << "CreateMutex error: " << GetLastError() << std::endl; @@ -101,7 +101,7 @@ class vpMutex #ifdef WINRT_8_1 dwWaitResult = WaitForSingleObjectEx(m_mutex, INFINITE, FALSE); #else - dwWaitResult = WaitForSingleObject(m_mutex, // handle to mutex + dwWaitResult = WaitForSingleObject(m_mutex, // handle to mutex INFINITE); // no time-out interval #endif if (dwWaitResult == WAIT_FAILED) @@ -130,8 +130,11 @@ class vpMutex \brief Class that allows protection by mutex. The following example shows how to use this class to protect a portion of -code from concurrent access. The scope of the mutex lock/unlock is determined -by the constructor/destructor. \code #include + code from concurrent access. The scope of the mutex lock/unlock is determined + by the constructor/destructor. + +\code + #include int main() { diff --git a/modules/core/include/visp3/core/vpNetwork.h b/modules/core/include/visp3/core/vpNetwork.h index ee0de7dcf6..2c866d33ef 100644 --- a/modules/core/include/visp3/core/vpNetwork.h +++ b/modules/core/include/visp3/core/vpNetwork.h @@ -47,8 +47,7 @@ #include #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #include #include #include @@ -62,7 +61,7 @@ #endif #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin) -#include // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro +#include // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro #endif /*! @@ -87,8 +86,7 @@ class VISP_EXPORT vpNetwork protected: #ifndef DOXYGEN_SHOULD_SKIP_THIS struct vpReceptor { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX int socketFileDescriptorReceptor; socklen_t receptorAddressSize; #else @@ -98,17 +96,12 @@ class VISP_EXPORT vpNetwork struct sockaddr_in receptorAddress; std::string receptorIP; - vpReceptor() - : socketFileDescriptorReceptor(0), receptorAddressSize(), - receptorAddress(), 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 +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX int socketFileDescriptorEmitter; #else SOCKET socketFileDescriptorEmitter; @@ -130,8 +123,7 @@ class VISP_EXPORT vpNetwork vpEmitter emitter; std::vector receptor_list; fd_set readFileDescriptor; -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX int socketMax; #else SOCKET socketMax; @@ -196,11 +188,9 @@ class VISP_EXPORT vpNetwork void print(const char *id = ""); + template int receive(T *object, const unsigned int &sizeOfObject = sizeof(T)); template - int receive(T *object, const unsigned int &sizeOfObject = sizeof(T)); - template - int receiveFrom(T *object, const unsigned int &receptorEmitting, - const unsigned int &sizeOfObject = sizeof(T)); + int receiveFrom(T *object, const unsigned int &receptorEmitting, const unsigned int &sizeOfObject = sizeof(T)); std::vector receiveRequest(); std::vector receiveRequestFrom(const unsigned int &receptorEmitting); @@ -208,18 +198,14 @@ class VISP_EXPORT vpNetwork int receiveRequestOnceFrom(const unsigned int &receptorEmitting); std::vector receiveAndDecodeRequest(); - std::vector - receiveAndDecodeRequestFrom(const unsigned int &receptorEmitting); + std::vector receiveAndDecodeRequestFrom(const unsigned int &receptorEmitting); int receiveAndDecodeRequestOnce(); int receiveAndDecodeRequestOnceFrom(const unsigned int &receptorEmitting); void removeDecodingRequest(const char *); - template - int send(T *object, const int unsigned &sizeOfObject = sizeof(T)); - template - int sendTo(T *object, const unsigned int &dest, - const unsigned int &sizeOfObject = sizeof(T)); + template int send(T *object, const int unsigned &sizeOfObject = sizeof(T)); + template 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); @@ -234,10 +220,7 @@ class VISP_EXPORT vpNetwork \param s : new maximum size value. */ - void setMaxSizeReceivedMessage(const unsigned int &s) - { - max_size_message = s; - } + 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 @@ -291,8 +274,7 @@ class VISP_EXPORT vpNetwork \return the number of bytes received, or -1 if an error occured. */ -template -int vpNetwork::receive(T *object, const unsigned int &sizeOfObject) +template int vpNetwork::receive(T *object, const unsigned int &sizeOfObject) { if (receptor_list.size() == 0) { if (verboseMode) @@ -310,8 +292,7 @@ int vpNetwork::receive(T *object, const unsigned int &sizeOfObject) FD_ZERO(&readFileDescriptor); for (unsigned int i = 0; i < receptor_list.size(); i++) { - FD_SET((unsigned int)receptor_list[i].socketFileDescriptorReceptor, - &readFileDescriptor); + FD_SET((unsigned int)receptor_list[i].socketFileDescriptorReceptor, &readFileDescriptor); if (i == 0) socketMax = receptor_list[i].socketFileDescriptorReceptor; @@ -320,8 +301,7 @@ int vpNetwork::receive(T *object, const unsigned int &sizeOfObject) socketMax = receptor_list[i].socketFileDescriptorReceptor; } - int value = - select((int)socketMax + 1, &readFileDescriptor, NULL, NULL, &tv); + int value = select((int)socketMax + 1, &readFileDescriptor, NULL, NULL, &tv); int numbytes = 0; if (value == -1) { @@ -333,22 +313,15 @@ int vpNetwork::receive(T *object, const unsigned int &sizeOfObject) 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); + 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); + 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; + std::cout << "Disconnected : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl; receptor_list.erase(receptor_list.begin() + (int)i); return numbytes; } @@ -383,11 +356,9 @@ int vpNetwork::receive(T *object, const unsigned int &sizeOfObject) \return the number of bytes received, or -1 if an error occured. */ template -int vpNetwork::receiveFrom(T *object, const unsigned int &receptorEmitting, - const unsigned int &sizeOfObject) +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 (receptor_list.size() == 0 || receptorEmitting > (unsigned int)receptor_list.size() - 1) { if (verboseMode) vpTRACE("No receptor at the specified index"); return -1; @@ -403,12 +374,9 @@ int vpNetwork::receiveFrom(T *object, const unsigned int &receptorEmitting, FD_ZERO(&readFileDescriptor); socketMax = receptor_list[receptorEmitting].socketFileDescriptorReceptor; - FD_SET((unsigned int)receptor_list[receptorEmitting] - .socketFileDescriptorReceptor, - &readFileDescriptor); + FD_SET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor, &readFileDescriptor); - int value = - select((int)socketMax + 1, &readFileDescriptor, NULL, NULL, &tv); + int value = select((int)socketMax + 1, &readFileDescriptor, NULL, NULL, &tv); int numbytes = 0; if (value == -1) { @@ -419,23 +387,16 @@ int vpNetwork::receiveFrom(T *object, const unsigned int &receptorEmitting, // 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 + 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); + recv(receptor_list[receptorEmitting].socketFileDescriptorReceptor, (char *)(void *)object, sizeOfObject, 0); #else - numbytes = recv((unsigned int)receptor_list[receptorEmitting] - .socketFileDescriptorReceptor, + 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::cout << "Disconnected : " << inet_ntoa(receptor_list[receptorEmitting].receptorAddress.sin_addr) << std::endl; receptor_list.erase(receptor_list.begin() + (int)receptorEmitting); return numbytes; @@ -467,8 +428,7 @@ int vpNetwork::receiveFrom(T *object, const unsigned int &receptorEmitting, \return The number of bytes sent, or -1 if an error happened. */ -template -int vpNetwork::send(T *object, const unsigned int &sizeOfObject) +template int vpNetwork::send(T *object, const unsigned int &sizeOfObject) { if (receptor_list.size() == 0) { if (verboseMode) @@ -482,17 +442,12 @@ int vpNetwork::send(T *object, const unsigned int &sizeOfObject) 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); +#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); + return sendto(receptor_list[0].socketFileDescriptorReceptor, (const char *)(void *)object, (int)sizeOfObject, flags, + (sockaddr *)&receptor_list[0].receptorAddress, receptor_list[0].receptorAddressSize); #endif } @@ -517,12 +472,9 @@ int vpNetwork::send(T *object, const unsigned int &sizeOfObject) \return The number of bytes sent, or -1 if an error happened. */ -template -int vpNetwork::sendTo(T *object, const unsigned int &dest, - const unsigned int &sizeOfObject) +template 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 (receptor_list.size() == 0 || dest > (unsigned int)receptor_list.size() - 1) { if (verboseMode) vpTRACE("No receptor at the specified index."); return 0; @@ -534,17 +486,12 @@ int vpNetwork::sendTo(T *object, const unsigned int &dest, 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); +#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); + return sendto(receptor_list[dest].socketFileDescriptorReceptor, (const char *)(void *)object, (int)sizeOfObject, + flags, (sockaddr *)&receptor_list[dest].receptorAddress, receptor_list[dest].receptorAddressSize); #endif } diff --git a/modules/core/include/visp3/core/vpPixelMeterConversion.h b/modules/core/include/visp3/core/vpPixelMeterConversion.h index 3d115ef93a..721ece3a6d 100644 --- a/modules/core/include/visp3/core/vpPixelMeterConversion.h +++ b/modules/core/include/visp3/core/vpPixelMeterConversion.h @@ -87,9 +87,7 @@ class VISP_EXPORT vpPixelMeterConversion 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) + inline static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y) { switch (cam.projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: @@ -125,9 +123,7 @@ class VISP_EXPORT vpPixelMeterConversion 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) + inline static void convertPoint(const vpCameraParameters &cam, const vpImagePoint &iP, double &x, double &y) { switch (cam.projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: @@ -151,10 +147,8 @@ class VISP_EXPORT vpPixelMeterConversion \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) + 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; @@ -175,9 +169,8 @@ class VISP_EXPORT vpPixelMeterConversion \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) + 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; @@ -197,13 +190,10 @@ class VISP_EXPORT vpPixelMeterConversion \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) + 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)); + 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; } @@ -224,24 +214,20 @@ class VISP_EXPORT vpPixelMeterConversion \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) + 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)); + 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, + 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, + static void convertMoment(const vpCameraParameters &cam, unsigned int order, const vpMatrix &moment_pixel, vpMatrix &moment_meter); }; diff --git a/modules/core/include/visp3/core/vpPlane.h b/modules/core/include/visp3/core/vpPlane.h index 31b185197b..0315ab6cc4 100644 --- a/modules/core/include/visp3/core/vpPlane.h +++ b/modules/core/include/visp3/core/vpPlane.h @@ -71,12 +71,9 @@ class VISP_EXPORT vpPlane 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); + 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); @@ -90,8 +87,7 @@ class VISP_EXPORT vpPlane /*! 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) + inline void setABCD(const double a, const double b, const double c, const double d) { this->A = a; this->B = b; @@ -156,8 +152,7 @@ class VISP_EXPORT vpPlane // Operation with Plane void projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const; - double rayIntersection(const vpPoint &M0, const vpPoint &M1, - vpColVector &H) 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); diff --git a/modules/core/include/visp3/core/vpPoint.h b/modules/core/include/visp3/core/vpPoint.h index 0f25c50a55..9e09430654 100644 --- a/modules/core/include/visp3/core/vpPoint.h +++ b/modules/core/include/visp3/core/vpPoint.h @@ -72,17 +72,12 @@ class VISP_EXPORT vpPoint : public vpForwardProjection void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP); void changeFrame(const vpHomogeneousMatrix &cMo); - void display(const vpImage &I, const vpCameraParameters &cam, - const vpColor &color = vpColor::green, - const unsigned int thickness = 1); - void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &color = vpColor::green, - const unsigned int thickness = 1); - void display(const vpImage &I, const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const vpColor &color = vpColor::green, + void display(const vpImage &I, const vpCameraParameters &cam, const vpColor &color = vpColor::green, const unsigned int thickness = 1); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &color = vpColor::green, const unsigned int thickness = 1); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &color = vpColor::green, const unsigned int thickness = 1); vpPoint *duplicate() const; // Get coordinates @@ -106,8 +101,7 @@ class VISP_EXPORT vpPoint : public vpForwardProjection //! Basic construction. void init(); - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpPoint &vpp); + 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 diff --git a/modules/core/include/visp3/core/vpPolygon.h b/modules/core/include/visp3/core/vpPolygon.h index 54c3e55734..64db07220d 100644 --- a/modules/core/include/visp3/core/vpPolygon.h +++ b/modules/core/include/visp3/core/vpPolygon.h @@ -83,8 +83,8 @@ int main() // Get the polygon bounding box vpRect bbox = polygon.getBoundingBox(); - std::cout << "Bounding box: " << bbox.getTopLeft() << " to " << -bbox.getBottomRight() << std::endl; + std::cout << "Bounding box: " << bbox.getTopLeft() << " to " + << bbox.getBottomRight() << std::endl; // Get the polygon surface and center std::cout << "Area: " << polygon.getArea() << std::endl; @@ -92,8 +92,9 @@ bbox.getBottomRight() << 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; + std::cout << "The point " << ip << " is " + << (polygon.isInside(ip) ? "inside":"outside") + << " the polygon" << std::endl; return 0; } @@ -118,8 +119,8 @@ class VISP_EXPORT vpPolygon public: enum PointInPolygonMethod { PnPolySegmentIntersection, /*!< Legacy Point In Polygon test. */ - PnPolyRayCasting /*!< Point In Polygon test using ray casting method - (faster). */ + PnPolyRayCasting /*!< Point In Polygon test using ray casting method + (faster). */ }; vpPolygon(); @@ -132,22 +133,17 @@ class VISP_EXPORT vpPolygon void buildFrom(const std::vector &corners); void buildFrom(const std::list &corners); - void buildFrom(const std::vector &corners, - const vpCameraParameters &cam); + void buildFrom(const std::vector &corners, const vpCameraParameters &cam); unsigned int getSize() const; - void initClick(const vpImage &I, unsigned int size = 5, - const vpColor &color = vpColor::red, + void initClick(const vpImage &I, unsigned int size = 5, const vpColor &color = vpColor::red, unsigned int thickness = 1); - void initClick(const vpImage &I, unsigned int size = 5, - const vpColor &color = vpColor::red, + void initClick(const vpImage &I, unsigned int size = 5, const vpColor &color = vpColor::red, unsigned int thickness = 1); - bool isInside(const vpImagePoint &iP, - const PointInPolygonMethod &method = PnPolyRayCasting) const; + bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method = PnPolyRayCasting) const; - void display(const vpImage &I, const vpColor &color, - unsigned int thickness = 1) const; + void display(const vpImage &I, const vpColor &color, unsigned int thickness = 1) const; /*! Get the corners of the polygon. @@ -188,9 +184,7 @@ class VISP_EXPORT vpPolygon void updateBoundingBox(); private: - bool testIntersectionSegments(const vpImagePoint &ip1, - const vpImagePoint &ip2, - const vpImagePoint &ip3, + bool testIntersectionSegments(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpImagePoint &ip3, const vpImagePoint &ip4) const; void precalcValuesPnPoly(); @@ -202,8 +196,7 @@ class VISP_EXPORT vpPolygon //################### public: - static bool isInside(const std::vector &roi, const double &i, - const double &j, + static bool isInside(const std::vector &roi, const double &i, const double &j, const PointInPolygonMethod &method = PnPolyRayCasting); }; diff --git a/modules/core/include/visp3/core/vpPolygon3D.h b/modules/core/include/visp3/core/vpPolygon3D.h index 8a0ea18921..47d250499f 100644 --- a/modules/core/include/visp3/core/vpPolygon3D.h +++ b/modules/core/include/visp3/core/vpPolygon3D.h @@ -89,18 +89,12 @@ class VISP_EXPORT vpPolygon3D double distFarClip; private: - bool getClippedPointsFovGeneric(const vpPoint &p1, const vpPoint &p2, - vpPoint &p1Clipped, vpPoint &p2Clipped, - unsigned int &p1ClippedInfo, - unsigned int &p2ClippedInfo, - const vpColVector &normal, + 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, + 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: @@ -112,8 +106,7 @@ class VISP_EXPORT vpPolygon3D void changeFrame(const vpHomogeneousMatrix &cMo); - void - computePolygonClipped(const vpCameraParameters &cam = vpCameraParameters()); + void computePolygonClipped(const vpCameraParameters &cam = vpCameraParameters()); /*! Get the clipping used. @@ -143,13 +136,9 @@ class VISP_EXPORT vpPolygon3D \return number of corner of the face at the previous computation */ - inline unsigned int getNbCornerInsidePrevImage() const - { - return nbCornersInsidePrev; - } + inline unsigned int getNbCornerInsidePrevImage() const { return nbCornersInsidePrev; } - unsigned int getNbCornerInsideImage(const vpImage &I, - const vpCameraParameters &cam); + unsigned int getNbCornerInsideImage(const vpImage &I, const vpCameraParameters &cam); /*! Get the near distance for clipping. @@ -162,22 +151,15 @@ class VISP_EXPORT vpPolygon3D std::vector getRoi(const vpCameraParameters &cam); - std::vector getRoi(const vpCameraParameters &cam, - const vpHomogeneousMatrix &cMo); + std::vector getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo); - void getRoiClipped(const vpCameraParameters &cam, - std::vector &roi); + void getRoiClipped(const vpCameraParameters &cam, std::vector &roi); - void getRoiClipped(const vpCameraParameters &cam, - std::vector &roi, - const vpHomogeneousMatrix &cMo); + void getRoiClipped(const vpCameraParameters &cam, std::vector &roi, const vpHomogeneousMatrix &cMo); - void - getRoiClipped(const vpCameraParameters &cam, - std::vector > &roi); + void getRoiClipped(const vpCameraParameters &cam, std::vector > &roi); - void getRoiClipped(const vpCameraParameters &cam, - std::vector > &roi, + void getRoiClipped(const vpCameraParameters &cam, std::vector > &roi, const vpHomogeneousMatrix &cMo); #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS @@ -189,8 +171,7 @@ class VISP_EXPORT vpPolygon3D //@} #endif - void - getPolygonClipped(std::vector > &poly); + void getPolygonClipped(std::vector > &poly); void getPolygonClipped(std::vector &poly); @@ -230,15 +211,12 @@ class VISP_EXPORT vpPolygon3D } public: - static void getClippedPolygon( - const std::vector &ptIn, std::vector &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 &roi, int &i_min, - int &i_max, int &j_min, int &j_max); - static bool roiInsideImage(const vpImage &I, - const std::vector &corners); + static void getClippedPolygon(const std::vector &ptIn, std::vector &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 &roi, int &i_min, int &i_max, int &j_min, int &j_max); + static bool roiInsideImage(const vpImage &I, const std::vector &corners); }; #endif diff --git a/modules/core/include/visp3/core/vpPoseVector.h b/modules/core/include/visp3/core/vpPoseVector.h index ada39b0e21..27fbf45d5d 100644 --- a/modules/core/include/visp3/core/vpPoseVector.h +++ b/modules/core/include/visp3/core/vpPoseVector.h @@ -95,8 +95,7 @@ class VISP_EXPORT vpPoseVector : public vpArray2D // 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); + 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 explicit vpPoseVector(const vpHomogeneousMatrix &M); // constructor convert a translation and a "thetau" vector into a pose @@ -108,17 +107,14 @@ class VISP_EXPORT vpPoseVector : public vpArray2D */ virtual ~vpPoseVector(){}; - vpPoseVector buildFrom(const double tx, const double ty, const double tz, - const double tux, const double tuy, + 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); + 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); + vpPoseVector buildFrom(const vpTranslationVector &tv, const vpRotationMatrix &R); void extract(vpRotationMatrix &R) const; void extract(vpThetaUVector &tu) const; @@ -175,35 +171,28 @@ class VISP_EXPORT vpPoseVector : public vpArray2D tuz = r[5]; \endcode */ - inline const double &operator[](unsigned int i) const - { - return *(data + i); - } + 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; + 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 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")); + 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); + 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) diff --git a/modules/core/include/visp3/core/vpQuaternionVector.h b/modules/core/include/visp3/core/vpQuaternionVector.h index 7a7a48fc07..d28226a68f 100644 --- a/modules/core/include/visp3/core/vpQuaternionVector.h +++ b/modules/core/include/visp3/core/vpQuaternionVector.h @@ -84,8 +84,7 @@ class VISP_EXPORT vpQuaternionVector : public vpRotationVector public: vpQuaternionVector(); vpQuaternionVector(const vpQuaternionVector &q); - vpQuaternionVector(const double qx, const double qy, const double qz, - const double qw); + vpQuaternionVector(const double qx, const double qy, const double qz, const double qw); explicit vpQuaternionVector(const vpRotationMatrix &R); explicit vpQuaternionVector(const vpThetaUVector &tu); explicit vpQuaternionVector(const vpColVector &q); @@ -93,8 +92,7 @@ class VISP_EXPORT vpQuaternionVector : public vpRotationVector //! Destructor. virtual ~vpQuaternionVector(){}; - vpQuaternionVector buildFrom(const double qx, const double qy, - const double qz, const double qw); + 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); diff --git a/modules/core/include/visp3/core/vpRGBa.h b/modules/core/include/visp3/core/vpRGBa.h index bfb4bff459..79cb146f24 100644 --- a/modules/core/include/visp3/core/vpRGBa.h +++ b/modules/core/include/visp3/core/vpRGBa.h @@ -87,8 +87,7 @@ class VISP_EXPORT vpRGBa \param a : Additional value. */ - inline vpRGBa(const unsigned char &r, const unsigned char &g, - const unsigned char &b, const unsigned char &a = 0) + 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) { } @@ -117,11 +116,7 @@ class VISP_EXPORT vpRGBa A=v[3] */ - inline vpRGBa(const vpColVector &v) - : R(0), G(0), B(0), A(vpRGBa::alpha_default) - { - *this = v; - } + inline vpRGBa(const vpColVector &v) : R(0), G(0), B(0), A(vpRGBa::alpha_default) { *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 @@ -147,8 +142,7 @@ class VISP_EXPORT vpRGBa bool operator<(const vpRGBa &v) const; bool operator>(const vpRGBa &v) const; - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpRGBa &rgba); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpRGBa &rgba); public: unsigned char R; //!< Red component. diff --git a/modules/core/include/visp3/core/vpRansac.h b/modules/core/include/visp3/core/vpRansac.h index 83545a3e93..47931c87ce 100644 --- a/modules/core/include/visp3/core/vpRansac.h +++ b/modules/core/include/visp3/core/vpRansac.h @@ -72,9 +72,8 @@ template class vpRansac { public: - static bool ransac(unsigned int npts, vpColVector &x, unsigned int s, - double t, vpColVector &model, vpColVector &inliers, - int consensus = 1000, double not_used = 0.0, + static bool ransac(unsigned int npts, vpColVector &x, unsigned int s, double t, vpColVector &model, + vpColVector &inliers, int consensus = 1000, double not_used = 0.0, const int maxNbumbersOfTrials = 10000); }; @@ -109,10 +108,8 @@ template class vpRansac */ template -bool vpRansac::ransac(unsigned int npts, vpColVector &x, - unsigned int s, double t, - vpColVector &M, vpColVector &inliers, - int consensus, double not_used, +bool vpRansac::ransac(unsigned int npts, vpColVector &x, unsigned int s, double t, vpColVector &M, + vpColVector &inliers, int consensus, double not_used, const int maxNbumbersOfTrials) { /* bool isplanar; */ @@ -123,10 +120,8 @@ bool vpRansac::ransac(unsigned int npts, vpColVector &x, 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 + 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) @@ -168,8 +163,7 @@ bool vpRansac::ransac(unsigned int npts, vpColVector &x, 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")); + throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set")); // return false; //Useless after a throw() function } } @@ -207,10 +201,8 @@ bool vpRansac::ransac(unsigned int npts, vpColVector &x, double pNoOutliers = 1 - pow(fracinliers, static_cast(s)); - pNoOutliers = - vpMath::maximum(eps, pNoOutliers); // Avoid division by -Inf - pNoOutliers = - vpMath::minimum(1 - eps, pNoOutliers); // Avoid division by 0. + 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)); } diff --git a/modules/core/include/visp3/core/vpRect.h b/modules/core/include/visp3/core/vpRect.h index 63bc3fd3ab..ec677a6ad1 100644 --- a/modules/core/include/visp3/core/vpRect.h +++ b/modules/core/include/visp3/core/vpRect.h @@ -206,10 +206,8 @@ class VISP_EXPORT vpRect vpRect &operator&=(const vpRect &r); vpRect operator&(const vpRect &r) const; - friend VISP_EXPORT bool inRectangle(const vpImagePoint &ip, - const vpRect &rect); - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpRect &r); + 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); diff --git a/modules/core/include/visp3/core/vpRequest.h b/modules/core/include/visp3/core/vpRequest.h index 187f3afa2c..b93698f964 100644 --- a/modules/core/include/visp3/core/vpRequest.h +++ b/modules/core/include/visp3/core/vpRequest.h @@ -119,8 +119,7 @@ void vpRequestImage::decode(){ 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)); + memcpy((void*)I->bitmap,(void*)listOfParams[2].c_str(),w*h*sizeof(unsigned char)); } } \endcode @@ -142,8 +141,7 @@ class VISP_EXPORT vpRequest void addParameter(char *params); void addParameter(std::string ¶ms); void addParameter(std::vector &listOfparams); - template - void addParameterObject(T *params, const int &sizeOfObject = sizeof(T)); + template void addParameterObject(T *params, const int &sizeOfObject = sizeof(T)); /*! Decode the parameters of the request (Funtion that has to be redifined). @@ -169,20 +167,14 @@ class VISP_EXPORT vpRequest \return Parameter at the index i. */ - inline std::string &operator[](const unsigned int &i) - { - return listOfParams[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]; - } + inline const std::string &operator[](const unsigned int &i) const { return listOfParams[i]; } /*! Get the ID of the request. @@ -226,8 +218,7 @@ class VISP_EXPORT vpRequest \param params : Object to add. \param sizeOfObject : Size of the object. */ -template -void vpRequest::addParameterObject(T *params, const int &sizeOfObject) +template void vpRequest::addParameterObject(T *params, const int &sizeOfObject) { if (sizeOfObject != 0) { char *tempS = new char[sizeOfObject]; diff --git a/modules/core/include/visp3/core/vpRobust.h b/modules/core/include/visp3/core/vpRobust.h index 9d7e7a8b00..b2624e9122 100644 --- a/modules/core/include/visp3/core/vpRobust.h +++ b/modules/core/include/visp3/core/vpRobust.h @@ -91,13 +91,11 @@ class VISP_EXPORT vpRobust virtual ~vpRobust(){}; //! Compute the weights according a residue vector and a PsiFunction - void MEstimator(const vpRobustEstimatorType method, - const vpColVector &residues, vpColVector &weights); + 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); + void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, const vpColVector &all_residues, + vpColVector &weights); vpRobust &operator=(const vpRobust &other); #ifdef VISP_HAVE_CPP11_COMPATIBILITY @@ -114,10 +112,7 @@ class VISP_EXPORT vpRobust Set maximal noise threshold. \param noise_threshold : Maximal noise threshold. */ - inline void setThreshold(const double noise_threshold) - { - NoiseThreshold = noise_threshold; - } + inline void setThreshold(const double noise_threshold) { NoiseThreshold = noise_threshold; } //! Simult Mestimator vpColVector simultMEstimator(vpColVector &residues); @@ -131,9 +126,7 @@ class VISP_EXPORT vpRobust private: //! Compute normalized median - double computeNormalizedMedian(vpColVector &all_normres, - const vpColVector &residues, - const vpColVector &all_residues, + double computeNormalizedMedian(vpColVector &all_normres, const vpColVector &residues, const vpColVector &all_residues, const vpColVector &weights); //! Calculate various scale estimates diff --git a/modules/core/include/visp3/core/vpRotationMatrix.h b/modules/core/include/visp3/core/vpRotationMatrix.h index da9fe09778..82d7f75e89 100644 --- a/modules/core/include/visp3/core/vpRotationMatrix.h +++ b/modules/core/include/visp3/core/vpRotationMatrix.h @@ -93,8 +93,7 @@ class VISP_EXPORT vpRotationMatrix : public vpArray2D 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); + vpRotationMatrix buildFrom(const double tux, const double tuy, const double tuz); void eye(); @@ -128,14 +127,12 @@ class VISP_EXPORT vpRotationMatrix : public vpArray2D 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 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")); + throw(vpException(vpException::fatalError, "Cannot resize a rotation matrix")); }; // transpose diff --git a/modules/core/include/visp3/core/vpRotationVector.h b/modules/core/include/visp3/core/vpRotationVector.h index 05a30126f8..748edc17c9 100644 --- a/modules/core/include/visp3/core/vpRotationVector.h +++ b/modules/core/include/visp3/core/vpRotationVector.h @@ -80,9 +80,12 @@ vpRxyzVector. 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 + // Rotation around x set to 45 degres converted in radians + r[0] = vpMath::rad(45); + // Rotation around y set to PI radians + r[1] = M_PI; + // Rotation around z set to 0 radians + r[2] = 0; std::cout << "Rxyz rotation vector: " << r << std::endl; @@ -126,10 +129,7 @@ class VISP_EXPORT vpRotationVector : public vpArray2D 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); - } + inline const double &operator[](unsigned int i) const { return *(data + i); } /*! Affectation of two vectors. diff --git a/modules/core/include/visp3/core/vpRowVector.h b/modules/core/include/visp3/core/vpRowVector.h index d09c0701c7..4b51eddfc4 100644 --- a/modules/core/include/visp3/core/vpRowVector.h +++ b/modules/core/include/visp3/core/vpRowVector.h @@ -110,9 +110,7 @@ class VISP_EXPORT vpRowVector : public vpArray2D rowNum = colNum = dsize = 0; } - std::ostream &cppPrint(std::ostream &os, - const std::string &matrixName = "A", - bool octet = false) const; + std::ostream &cppPrint(std::ostream &os, const std::string &matrixName = "A", bool octet = false) const; std::ostream &csvPrint(std::ostream &os) const; /*! @@ -167,10 +165,7 @@ class VISP_EXPORT vpRowVector : public vpArray2D //! 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); - } + inline const double &operator[](unsigned int n) const { return *(data + n); } //! Copy operator. Allow operation such as A = v vpRowVector &operator=(const vpRowVector &v); @@ -196,8 +191,7 @@ class VISP_EXPORT vpRowVector : public vpArray2D vpRowVector &operator<<(const vpRowVector &v); - int print(std::ostream &s, unsigned int length, - char const *intro = 0) const; + 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() @@ -208,8 +202,7 @@ class VISP_EXPORT vpRowVector : public vpArray2D (*this) *= r2d; } - void reshape(vpMatrix &M, const unsigned int &nrows, - const unsigned int &ncols); + 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. @@ -232,8 +225,7 @@ class VISP_EXPORT vpRowVector : public vpArray2D \exception vpException::fatalError When \e nrows is not equal to 1. */ - void resize(const unsigned int nrows, const unsigned int ncols, - const bool flagNullify) + void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify) { if (nrows != 1) throw(vpException(vpException::fatalError, @@ -255,10 +247,8 @@ class VISP_EXPORT vpRowVector : public vpArray2D 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); + 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) /*! @@ -278,17 +268,12 @@ class VISP_EXPORT vpRowVector : public vpArray2D \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); - } + 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) + vp_deprecated static void stackMatrices(const vpRowVector &A, const vpRowVector &B, vpRowVector &C) { stack(A, B, C); } diff --git a/modules/core/include/visp3/core/vpScale.h b/modules/core/include/visp3/core/vpScale.h index 4d430642fc..59b93585c9 100644 --- a/modules/core/include/visp3/core/vpScale.h +++ b/modules/core/include/visp3/core/vpScale.h @@ -45,8 +45,8 @@ /*! * \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 + * MPDE, Mean shift kernel + * density estimation. \author Andrew Comport \date 24/10/03 */ // ========================================================== diff --git a/modules/core/include/visp3/core/vpServer.h b/modules/core/include/visp3/core/vpServer.h index b2ad3a5d66..8147351e48 100644 --- a/modules/core/include/visp3/core/vpServer.h +++ b/modules/core/include/visp3/core/vpServer.h @@ -76,14 +76,18 @@ int main(int argc,const char** argv) 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; + // Receiving a value from the first client + if(serv.receive(&val) != sizeof(int)) + 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; + // Sending the new value to the first client + if(serv.send(&val) != sizeof(int)) + std::cout << "Error while sending" << std::endl; + else + std::cout << "Sending : " << val << std::endl; } } @@ -199,10 +203,7 @@ class VISP_EXPORT vpServer : public vpNetwork \return Number of clients connected. */ - unsigned int getNumberOfClients() - { - return (unsigned int)receptor_list.size(); - } + unsigned int getNumberOfClients() { return (unsigned int)receptor_list.size(); } void print(); diff --git a/modules/core/include/visp3/core/vpSphere.h b/modules/core/include/visp3/core/vpSphere.h index 3bfc0effde..4a68445a9c 100644 --- a/modules/core/include/visp3/core/vpSphere.h +++ b/modules/core/include/visp3/core/vpSphere.h @@ -69,8 +69,7 @@ class VISP_EXPORT vpSphere : public vpForwardProjection 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); + 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]; } @@ -89,13 +88,10 @@ class VISP_EXPORT vpSphere : public vpForwardProjection void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP); void changeFrame(const vpHomogeneousMatrix &cMo); - void display(const vpImage &I, const vpCameraParameters &cam, - const vpColor &color = vpColor::green, - const unsigned int thickness = 1); - void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &color = vpColor::green, + void display(const vpImage &I, const vpCameraParameters &cam, const vpColor &color = vpColor::green, const unsigned int thickness = 1); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &color = vpColor::green, const unsigned int thickness = 1); vpSphere *duplicate() const; }; diff --git a/modules/core/include/visp3/core/vpSubColVector.h b/modules/core/include/visp3/core/vpSubColVector.h index bc7938f10e..eeb69d10e1 100644 --- a/modules/core/include/visp3/core/vpSubColVector.h +++ b/modules/core/include/visp3/core/vpSubColVector.h @@ -74,14 +74,12 @@ class VISP_EXPORT vpSubColVector : public vpColVector public: vpSubColVector(); - vpSubColVector(vpColVector &v, const unsigned int &offset, - const unsigned int &nrows); + 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); + void init(vpColVector &v, const unsigned int &offset, const unsigned int &nrows); vpSubColVector &operator=(const vpSubColVector &B); diff --git a/modules/core/include/visp3/core/vpSubMatrix.h b/modules/core/include/visp3/core/vpSubMatrix.h index ebee040c88..f38378ecc5 100644 --- a/modules/core/include/visp3/core/vpSubMatrix.h +++ b/modules/core/include/visp3/core/vpSubMatrix.h @@ -79,14 +79,14 @@ class VISP_EXPORT vpSubMatrix : public vpMatrix //! Default constructor vpSubMatrix(); //! Constructor - vpSubMatrix(vpMatrix &m, const unsigned int &row, const unsigned int &col, - const unsigned int &nrows, const unsigned int &ncols); + 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); + 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; diff --git a/modules/core/include/visp3/core/vpSubRowVector.h b/modules/core/include/visp3/core/vpSubRowVector.h index 43552796a8..951cd5854e 100644 --- a/modules/core/include/visp3/core/vpSubRowVector.h +++ b/modules/core/include/visp3/core/vpSubRowVector.h @@ -75,14 +75,12 @@ class VISP_EXPORT vpSubRowVector : public vpRowVector public: vpSubRowVector(); - vpSubRowVector(vpRowVector &v, const unsigned int &offset, - const unsigned int &ncols); + 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); + void init(vpRowVector &v, const unsigned int &offset, const unsigned int &ncols); vpSubRowVector &operator=(const vpSubRowVector &B); vpSubRowVector &operator=(const vpRowVector &B); diff --git a/modules/core/include/visp3/core/vpThread.h b/modules/core/include/visp3/core/vpThread.h index d43c5067f3..26af6110c5 100644 --- a/modules/core/include/visp3/core/vpThread.h +++ b/modules/core/include/visp3/core/vpThread.h @@ -60,8 +60,7 @@ class vpThread arguments. \param fn : A pointer to a function. \param args : Arguments passed to the call to \e fn (if any). */ - vpThread(vpThread::Fn fn, vpThread::Args args = NULL) - : m_handle(), m_isCreated(false), m_isJoinable(false) + vpThread(vpThread::Fn fn, vpThread::Args args = NULL) : m_handle(), m_isCreated(false), m_isJoinable(false) { create(fn, args); } @@ -74,23 +73,20 @@ class vpThread void create(vpThread::Fn fn, vpThread::Args args = NULL) { if (m_isCreated) - throw vpException(vpException::fatalError, - "The thread is already created"); + throw vpException(vpException::fatalError, "The thread is already created"); #if defined(VISP_HAVE_PTHREAD) int err = pthread_create(&m_handle, NULL, fn, args); if (err != 0) { - throw vpException(vpException::cannotUseConstructorError, - "Can't create thread : %s", strerror(err)); + throw vpException(vpException::cannotUseConstructorError, "Can't create thread : %s", strerror(err)); } #elif defined(_WIN32) DWORD dwThreadIdArray; - m_handle = - CreateThread(NULL, // default security attributes - 0, // use default stack size - fn, // thread function name - args, // argument to thread function - 0, // use default creation flags - &dwThreadIdArray); // returns the thread identifier + m_handle = CreateThread(NULL, // default security attributes + 0, // use default stack size + fn, // thread function name + args, // argument to thread function + 0, // use default creation flags + &dwThreadIdArray); // returns the thread identifier #endif m_isJoinable = true; diff --git a/modules/core/include/visp3/core/vpTime.h b/modules/core/include/visp3/core/vpTime.h index cecc804637..905b136b07 100644 --- a/modules/core/include/visp3/core/vpTime.h +++ b/modules/core/include/visp3/core/vpTime.h @@ -75,8 +75,7 @@ int main() namespace vpTime { -VISP_EXPORT std::string -getDateTime(const std::string &format = "%Y/%m/%d %H:%M:%S"); +VISP_EXPORT std::string getDateTime(const std::string &format = "%Y/%m/%d %H:%M:%S"); VISP_EXPORT double getMinTimeForUsleepCall(); VISP_EXPORT double measureTimeSecond(); VISP_EXPORT double measureTimeMs(); diff --git a/modules/core/include/visp3/core/vpTrackingException.h b/modules/core/include/visp3/core/vpTrackingException.h index 6d584fe12b..c9e24f4552 100644 --- a/modules/core/include/visp3/core/vpTrackingException.h +++ b/modules/core/include/visp3/core/vpTrackingException.h @@ -91,11 +91,7 @@ class VISP_EXPORT vpTrackingException : public vpException setMessage(format, args); va_end(args); } - vpTrackingException(const int id, const std::string &msg) - : vpException(id, msg) - { - ; - } + vpTrackingException(const int id, const std::string &msg) : vpException(id, msg) { ; } explicit vpTrackingException(const int id) : vpException(id) { ; } }; diff --git a/modules/core/include/visp3/core/vpTranslationVector.h b/modules/core/include/visp3/core/vpTranslationVector.h index dcd20173c4..8a16db2278 100644 --- a/modules/core/include/visp3/core/vpTranslationVector.h +++ b/modules/core/include/visp3/core/vpTranslationVector.h @@ -102,8 +102,7 @@ class VISP_EXPORT vpTranslationVector : public vpArray2D explicit vpTranslationVector(const vpPoseVector &p); explicit vpTranslationVector(const vpColVector &v); - vpTranslationVector buildFrom(const double tx, const double ty, - const double tz); + vpTranslationVector buildFrom(const double tx, const double ty, const double tz); vpTranslationVector buildFrom(const vpHomogeneousMatrix &M); vpTranslationVector buildFrom(const vpPoseVector &p); vpTranslationVector buildFrom(const vpColVector &v); @@ -134,24 +133,19 @@ class VISP_EXPORT vpTranslationVector : public vpArray2D //! 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); - } + 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 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")); + throw(vpException(vpException::fatalError, "Cannot resize a translation vector")); }; void set(const double tx, const double ty, const double tz); @@ -163,8 +157,7 @@ class VISP_EXPORT vpTranslationVector : public vpArray2D vpRowVector t() const; - static vpTranslationVector cross(const vpTranslationVector &a, - const vpTranslationVector &b); + static vpTranslationVector cross(const vpTranslationVector &a, const vpTranslationVector &b); static vpMatrix skew(const vpTranslationVector &tv); static void skew(const vpTranslationVector &tv, vpMatrix &M); }; diff --git a/modules/core/include/visp3/core/vpTriangle.h b/modules/core/include/visp3/core/vpTriangle.h index 8c5f9c84e5..c1a0bc3b39 100644 --- a/modules/core/include/visp3/core/vpTriangle.h +++ b/modules/core/include/visp3/core/vpTriangle.h @@ -78,11 +78,9 @@ class VISP_EXPORT vpTriangle vpTriangle &operator=(const vpTriangle &tri); - vpTriangle(const vpImagePoint &iP1, const vpImagePoint &iP2, - const vpImagePoint &iP3); + vpTriangle(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3); - void buildFrom(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); @@ -93,8 +91,7 @@ class VISP_EXPORT vpTriangle \param iP2 : second apex. \param iP3 : third apex. */ - void getTriangleApexes(vpImagePoint &iP1, vpImagePoint &iP2, - vpImagePoint &iP3) const + void getTriangleApexes(vpImagePoint &iP1, vpImagePoint &iP2, vpImagePoint &iP3) const { iP1 = apex1; iP2 = apex2; @@ -110,8 +107,7 @@ class VISP_EXPORT vpTriangle inline double getArea() const { return this->area; } private: - void init(const vpImagePoint &iP1, const vpImagePoint &iP2, - const vpImagePoint &iP3); + void init(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3); }; #endif diff --git a/modules/core/include/visp3/core/vpUDPClient.h b/modules/core/include/visp3/core/vpUDPClient.h index c6089b5653..11020541d6 100644 --- a/modules/core/include/visp3/core/vpUDPClient.h +++ b/modules/core/include/visp3/core/vpUDPClient.h @@ -38,8 +38,7 @@ #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #include #include #include @@ -121,14 +120,13 @@ receive): \code #include -#include #include +#include #include struct DataType { double double_val; int int_val; - DataType() : double_val(0.0), int_val(0) {} DataType(const double dbl, const int i) : double_val(dbl), int_val(i) {} }; @@ -138,25 +136,22 @@ int main() { std::string servername = "127.0.0.1"; unsigned int port = 50037; vpUDPClient client(servername, port); - DataType data_type(1234.56789, 123450); char data[sizeof(data_type.double_val)+sizeof(data_type.int_val)]; + memcpy(data, &data_type.double_val, sizeof(data_type.double_val)); - memcpy(data+sizeof(data_type.double_val), &data_type.int_val, -sizeof(data_type.int_val)); std::string msg(data, -sizeof(data_type.double_val)+sizeof(data_type.int_val)); if (client.send(msg) -!= (int) sizeof(data_type.double_val)+sizeof(data_type.int_val)) std::cerr << -"Error client.send()!" << std::endl; + memcpy(data+sizeof(data_type.double_val), &data_type.int_val, sizeof(data_type.int_val)); + std::string msg(data, sizeof(data_type.double_val)+sizeof(data_type.int_val)); + if (client.send(msg) != (int) sizeof(data_type.double_val)+sizeof(data_type.int_val)) + std::cerr << "Error client.send()!" << std::endl; if (client.receive(msg)) { data_type.double_val = *reinterpret_cast(msg.c_str()); - data_type.int_val = *reinterpret_cast(msg.c_str()+sizeof(data_type.double_val)); - - std::cout << "Receive from the server double_val: " << -data_type.double_val << " ; int_val: " << data_type.int_val << std::endl; + data_type.int_val + = *reinterpret_cast(msg.c_str()+sizeof(data_type.double_val)); + std::cout << "Receive from the server double_val: " << data_type.double_val + << " ; int_val: " << data_type.int_val << std::endl; } - return EXIT_SUCCESS; } catch (const vpException &e) { std::cerr << "Catch an exception: " << e.what() << std::endl; @@ -180,8 +175,7 @@ class VISP_EXPORT vpUDPClient char m_buf[VP_MAX_UDP_PAYLOAD]; struct sockaddr_in m_serverAddress; int m_serverLength; -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX int m_socketFileDescriptor; #else SOCKET m_socketFileDescriptor; diff --git a/modules/core/include/visp3/core/vpUDPServer.h b/modules/core/include/visp3/core/vpUDPServer.h index 81e8c6b32e..bfdced478a 100644 --- a/modules/core/include/visp3/core/vpUDPServer.h +++ b/modules/core/include/visp3/core/vpUDPServer.h @@ -38,8 +38,7 @@ #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #include #include #include @@ -98,8 +97,8 @@ int main() { std::string msg = "", hostInfo = ""; int res = server.receive(msg, hostInfo, 5000); if (res) { - std::cout << "Server received: " << msg << " from: " << hostInfo << -std::endl; std::cout << "Reply to the client: Echo: " << msg << std::endl; + std::cout << "Server received: " << msg << " from: " << hostInfo << std::endl; + std::cout << "Reply to the client: Echo: " << msg << std::endl; //Get address and port std::istringstream iss(hostInfo); @@ -200,18 +199,15 @@ class VISP_EXPORT vpUDPServer ~vpUDPServer(); int receive(std::string &msg, const int timeoutMs = 0); - int receive(std::string &msg, std::string &hostInfo, - const int timeoutMs = 0); - int send(const std::string &msg, const std::string &hostname, - const int port); + int receive(std::string &msg, std::string &hostInfo, const int timeoutMs = 0); + int send(const std::string &msg, const std::string &hostname, const int port); private: char m_buf[VP_MAX_UDP_PAYLOAD]; struct sockaddr_in m_clientAddress; int m_clientLength; struct sockaddr_in m_serverAddress; -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX int m_socketFileDescriptor; #else SOCKET m_socketFileDescriptor; diff --git a/modules/core/include/visp3/core/vpUniRand.h b/modules/core/include/visp3/core/vpUniRand.h index fd5c327fb6..c84aa2fd9f 100644 --- a/modules/core/include/visp3/core/vpUniRand.h +++ b/modules/core/include/visp3/core/vpUniRand.h @@ -49,8 +49,10 @@ 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 #include + The following example shows how to use this class to generate 10 numbers between 0 and 5. +\code +#include +#include int main() { @@ -79,8 +81,7 @@ class VISP_EXPORT vpUniRand public: //! Default constructor. explicit vpUniRand(const long seed = 0) - : a(16807), m(2147483647), q(127773), r(2836), normalizer(2147484721.0), - x((seed) ? seed : 739806647) + : a(16807), m(2147483647), q(127773), r(2836), normalizer(2147484721.0), x((seed) ? seed : 739806647) { } diff --git a/modules/core/include/visp3/core/vpVelocityTwistMatrix.h b/modules/core/include/visp3/core/vpVelocityTwistMatrix.h index 52b9194011..923d8000e5 100644 --- a/modules/core/include/visp3/core/vpVelocityTwistMatrix.h +++ b/modules/core/include/visp3/core/vpVelocityTwistMatrix.h @@ -121,17 +121,14 @@ class VISP_EXPORT vpVelocityTwistMatrix : public vpArray2D // copy constructor vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V); // constructor from an homogeneous transformation - explicit vpVelocityTwistMatrix(const vpHomogeneousMatrix &M, - bool full = true); + explicit vpVelocityTwistMatrix(const vpHomogeneousMatrix &M, bool full = true); // Construction from Translation and rotation (matrix parameterization) - vpVelocityTwistMatrix(const vpTranslationVector &t, - const vpRotationMatrix &R); + vpVelocityTwistMatrix(const vpTranslationVector &t, const vpRotationMatrix &R); // Construction from Translation and rotation (ThetaU parameterization) - vpVelocityTwistMatrix(const vpTranslationVector &t, - const vpThetaUVector &thetau); - vpVelocityTwistMatrix(const double tx, const double ty, const double tz, - const double tux, const double tuy, const double tuz); + vpVelocityTwistMatrix(const vpTranslationVector &t, const vpThetaUVector &thetau); + vpVelocityTwistMatrix(const double tx, const double ty, const double tz, const double tux, const double tuy, + const double tuz); vpVelocityTwistMatrix(const vpRotationMatrix &R); vpVelocityTwistMatrix(const vpThetaUVector &thetau); @@ -141,12 +138,9 @@ class VISP_EXPORT vpVelocityTwistMatrix : public vpArray2D */ virtual ~vpVelocityTwistMatrix(){}; - vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, - const vpRotationMatrix &R); - vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, - const vpThetaUVector &thetau); - vpVelocityTwistMatrix buildFrom(const vpHomogeneousMatrix &M, - bool full = true); + vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R); + vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpThetaUVector &thetau); + vpVelocityTwistMatrix buildFrom(const vpHomogeneousMatrix &M, bool full = true); vpVelocityTwistMatrix buildFrom(const vpRotationMatrix &R); vpVelocityTwistMatrix buildFrom(const vpThetaUVector &thetau); @@ -165,22 +159,19 @@ class VISP_EXPORT vpVelocityTwistMatrix : public vpArray2D vpVelocityTwistMatrix &operator=(const vpVelocityTwistMatrix &V); - int print(std::ostream &s, unsigned int length, - char const *intro = 0) const; + 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 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")); + throw(vpException(vpException::fatalError, "Cannot resize a velocity twist matrix")); }; #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) diff --git a/modules/core/include/visp3/core/vpXmlParser.h b/modules/core/include/visp3/core/vpXmlParser.h index dea389087c..8da542fa7b 100644 --- a/modules/core/include/visp3/core/vpXmlParser.h +++ b/modules/core/include/visp3/core/vpXmlParser.h @@ -128,27 +128,27 @@ void vpDataParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) { - for (xmlNodePtr tmpNode = node->xmlChildrenNode; tmpNode != NULL; tmpNode - = tmpNode->next) { if(tmpNode->type == XML_ELEMENT_NODE){ + for (xmlNodePtr tmpNode = node->xmlChildrenNode; tmpNode != NULL; tmpNode = tmpNode->next) { + if(tmpNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter= - this->nodeMap.find((char*)tmpNode->name); if(iter == nodeMap.end()){ + std::map::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; + this->m_range = xmlReadIntChild(doc, tmpNode); + break; case step: - this->m_step = xmlReadIntChild(doc, tmpNode); - break; + this->m_step = xmlReadIntChild(doc, tmpNode); + break; case size_filter: - this->m_size_filter = xmlReadIntChild(doc, tmpNode); - break; + this->m_size_filter = xmlReadIntChild(doc, tmpNode); + break; default: - std::cout << "problem in the readMainClass (" << iter->second - << " , " << iter->first << " )" << std::endl; break; + std::cout << "problem in the readMainClass (" << iter->second + << " , " << iter->first << " )" << std::endl; break; } } } @@ -210,19 +210,13 @@ class VISP_EXPORT vpXmlParser std::string xmlReadStringChild(xmlDocPtr doc, xmlNodePtr node); unsigned int xmlReadUnsignedIntChild(xmlDocPtr doc, xmlNodePtr node); - void xmlWriteBoolChild(xmlNodePtr node, const char *label, - const bool value); - void xmlWriteCharChild(xmlNodePtr node, const char *label, - const char *value); - void xmlWriteDoubleChild(xmlNodePtr node, const char *label, - const double value); - void xmlWriteFloatChild(xmlNodePtr node, const char *label, - const float value); + void xmlWriteBoolChild(xmlNodePtr node, const char *label, const bool value); + void xmlWriteCharChild(xmlNodePtr node, const char *label, const char *value); + void xmlWriteDoubleChild(xmlNodePtr node, const char *label, const double value); + void xmlWriteFloatChild(xmlNodePtr node, const char *label, const float value); void xmlWriteIntChild(xmlNodePtr node, const char *label, const int value); - void xmlWriteStringChild(xmlNodePtr node, const char *label, - const std::string &value); - void xmlWriteUnsignedIntChild(xmlNodePtr node, const char *label, - const unsigned int value); + void xmlWriteStringChild(xmlNodePtr node, const char *label, const std::string &value); + void xmlWriteUnsignedIntChild(xmlNodePtr node, const char *label, const unsigned int value); //@} protected: @@ -244,8 +238,7 @@ class VISP_EXPORT vpXmlParser virtual ~vpXmlParser(); /* virtual */ void parse(const std::string &filename); - /* virtual */ void save(const std::string &filename, - const bool append = false); + /* 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 diff --git a/modules/core/include/visp3/core/vpXmlParserCamera.h b/modules/core/include/visp3/core/vpXmlParserCamera.h index 4f6c52e3bb..af47c7118e 100644 --- a/modules/core/include/visp3/core/vpXmlParserCamera.h +++ b/modules/core/include/visp3/core/vpXmlParserCamera.h @@ -119,9 +119,8 @@ model // 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; + if (p.parse(cam, "myXmlFile.xml", "myCamera", projModel,640,480) != vpXmlParserCamera::SEQUENCE_OK) { + std::cout << "Cannot found myCamera" << std::endl; } // cout the parameters @@ -175,9 +174,8 @@ int main() // 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; + 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 @@ -247,82 +245,56 @@ class VISP_EXPORT vpXmlParserCamera : public vpXmlParser vpXmlParserCamera &operator=(const vpXmlParserCamera &twinparser); - int parse(vpCameraParameters &cam, const std::string &filename, - const std::string &camera_name, - const vpCameraParameters::vpCameraParametersProjType &projModel, - const unsigned int image_width = 0, + 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, + 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, const std::string &additionalInfo = ""); void setCameraName(const std::string &name) { this->camera_name = name; } 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; - } + void setSubsampling_width(const unsigned int subsampling) { this->subsampling_width = subsampling; } + void setSubsampling_height(const unsigned int subsampling) { this->subsampling_height = subsampling; } void setWidth(const unsigned int width) { this->image_width = width; } 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 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 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); + 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); + 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); xmlNodePtr find_additional_info(xmlNodePtr node); - vpXmlCodeSequenceType read_camera_model(xmlDocPtr doc, xmlNodePtr node, - vpCameraParameters &camera); + 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); + 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 myXmlReadIntChild(xmlDocPtr doc, xmlNodePtr node, int &res, vpXmlCodeSequenceType &code_error); - void myXmlReadDoubleChild(xmlDocPtr doc, xmlNodePtr node, double &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, + 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); diff --git a/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h b/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h index 41f9daeff4..5077b57ab7 100644 --- a/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h +++ b/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h @@ -111,13 +111,12 @@ int main(int argc, char* argv[]) // 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; + 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 + std::cout << "Homogeneous matrix " << name <<": " << std::endl << eMc << std::endl; +#endif return 0; } @@ -152,9 +151,8 @@ int main(int argc, char* argv[]) 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; + if (p.save(M, filename, name_M) != vpXmlParserHomogeneousMatrix::SEQUENCE_OK) { + std::cout << "Cannot save the Homogeneous matrix" << std::endl; } vpXmlParser::cleanup(); @@ -200,18 +198,12 @@ class VISP_EXPORT vpXmlParserHomogeneousMatrix : public vpXmlParser 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); + 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); + int save(const vpHomogeneousMatrix &M, const std::string &filename, const std::string &name); - void setHomogeneousMatrixName(const std::string &name) - { - this->m_name = name; - } + void setHomogeneousMatrixName(const std::string &name) { this->m_name = name; } private: int read(xmlDocPtr doc, xmlNodePtr node, const std::string &name); @@ -220,15 +212,12 @@ class VISP_EXPORT vpXmlParserHomogeneousMatrix : public vpXmlParser int read_matrix(xmlDocPtr doc, xmlNodePtr node, const std::string &name); - vpXmlCodeSequenceType read_values(xmlDocPtr doc, xmlNodePtr node, - vpHomogeneousMatrix &M); + 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 myXmlReadIntChild(xmlDocPtr doc, xmlNodePtr node, int &res, vpXmlCodeSequenceType &code_error); - void myXmlReadDoubleChild(xmlDocPtr doc, xmlNodePtr node, double &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); diff --git a/modules/core/src/camera/vpCameraParameters.cpp b/modules/core/src/camera/vpCameraParameters.cpp index 1e58be6c23..14c4a603a4 100644 --- a/modules/core/src/camera/vpCameraParameters.cpp +++ b/modules/core/src/camera/vpCameraParameters.cpp @@ -60,9 +60,8 @@ 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; +const vpCameraParameters::vpCameraParametersProjType vpCameraParameters::DEFAULT_PROJ_TYPE = + vpCameraParameters::perspectiveProjWithoutDistortion; /*! Default constructor. @@ -71,11 +70,9 @@ const vpCameraParameters::vpCameraParametersProjType \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), + : 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(); @@ -85,11 +82,9 @@ vpCameraParameters::vpCameraParameters() 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), + : 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); @@ -102,15 +97,11 @@ vpCameraParameters::vpCameraParameters(const vpCameraParameters &c) \param cam_u0,cam_v0 : principal points */ -vpCameraParameters::vpCameraParameters(const double cam_px, - const double cam_py, - const double cam_u0, +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), + : 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); @@ -125,18 +116,14 @@ vpCameraParameters::vpCameraParameters(const double cam_px, \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), +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); + initPersProjWithDistortion(cam_px, cam_py, cam_u0, cam_v0, cam_kud, cam_kdu); } /*! @@ -146,13 +133,11 @@ void vpCameraParameters::init() { if (fabs(this->px) < 1e-6) { vpERROR_TRACE("Camera parameter px = 0"); - throw(vpException(vpException::divideByZeroError, - "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")); + throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0")); } this->inv_px = 1. / this->px; this->inv_py = 1. / this->py; @@ -180,10 +165,9 @@ int main() 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; + 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: @@ -196,9 +180,7 @@ 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, +void vpCameraParameters::initPersProjWithoutDistortion(const double cam_px, const double cam_py, const double cam_u0, const double cam_v0) { this->projModel = vpCameraParameters::perspectiveProjWithoutDistortion; @@ -212,13 +194,11 @@ void vpCameraParameters::initPersProjWithoutDistortion(const double cam_px, if (fabs(px) < 1e-6) { vpERROR_TRACE("Camera parameter px = 0"); - throw(vpException(vpException::divideByZeroError, - "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")); + throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0")); } this->inv_px = 1. / px; this->inv_py = 1. / py; @@ -227,10 +207,10 @@ void vpCameraParameters::initPersProjWithoutDistortion(const double cam_px, /*! 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. + \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 @@ -250,10 +230,9 @@ int main() 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; + 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: @@ -267,9 +246,8 @@ Camera parameters for perspective projection with distortion: 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) +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; @@ -282,13 +260,11 @@ void vpCameraParameters::initPersProjWithDistortion( if (fabs(px) < 1e-6) { vpERROR_TRACE("Camera parameter px = 0"); - throw(vpException(vpException::divideByZeroError, - "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")); + throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0")); } this->inv_px = 1. / px; this->inv_py = 1. / py; @@ -323,12 +299,10 @@ void vpCameraParameters::init(const vpCameraParameters &c) { *this = c; } void vpCameraParameters::initFromCalibrationMatrix(const vpMatrix &_K) { if (_K.getRows() != 3 || _K.getCols() != 3) { - throw vpException(vpException::dimensionError, - "bad size for calibration matrix"); + throw vpException(vpException::dimensionError, "bad size for calibration matrix"); } if (std::fabs(_K[2][2] - 1.0) > std::numeric_limits::epsilon()) { - throw vpException(vpException::badValue, - "bad value: K[2][2] must be equal to 1"); + 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]); } @@ -353,10 +327,8 @@ int main() 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; + 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: @@ -368,9 +340,8 @@ Camera parameters for perspective projection without distortion: 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) +void vpCameraParameters::initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, + const double &vfov) { projModel = vpCameraParameters::perspectiveProjWithoutDistortion; u0 = (double)w / 2.; @@ -387,8 +358,7 @@ void vpCameraParameters::initFromFov(const unsigned int &w, /*! copy operator */ -vpCameraParameters &vpCameraParameters:: -operator=(const vpCameraParameters &cam) +vpCameraParameters &vpCameraParameters::operator=(const vpCameraParameters &cam) { projModel = cam.projModel; px = cam.px; @@ -417,8 +387,7 @@ operator=(const vpCameraParameters &cam) \param w : Width of the image \param h : Height of the image. */ -void vpCameraParameters::computeFov(const unsigned int &w, - const unsigned int &h) +void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h) { if ((!isFov || w != width || h != height) && w != 0 && h != 0) { fovNormals = std::vector(4); @@ -523,17 +492,13 @@ void vpCameraParameters::printParameters() switch (projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: std::cout.precision(10); - std::cout - << "Camera parameters for perspective projection without distortion:" - << std::endl; + 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 << "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; @@ -550,27 +515,20 @@ void vpCameraParameters::printParameters() \param os : Output stream. \param cam : Camera parameters. */ -VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpCameraParameters &cam) +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; + 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 << "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; diff --git a/modules/core/src/camera/vpMeterPixelConversion.cpp b/modules/core/src/camera/vpMeterPixelConversion.cpp index 6fc0b8e5dd..e34cea3294 100644 --- a/modules/core/src/camera/vpMeterPixelConversion.cpp +++ b/modules/core/src/camera/vpMeterPixelConversion.cpp @@ -49,10 +49,8 @@ #include //! 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) +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); @@ -64,8 +62,7 @@ void vpMeterPixelConversion::convertLine(const vpCameraParameters &cam, } 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 = (cam.px * cam.py * rho_m + cam.u0 * cam.py * co + cam.v0 * cam.px * si); rho_p /= d; } @@ -88,15 +85,12 @@ void vpMeterPixelConversion::convertLine(const vpCameraParameters &cam, 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 + 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) +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]; diff --git a/modules/core/src/camera/vpPixelMeterConversion.cpp b/modules/core/src/camera/vpPixelMeterConversion.cpp index e60cecbd2d..02587b5f9e 100644 --- a/modules/core/src/camera/vpPixelMeterConversion.cpp +++ b/modules/core/src/camera/vpPixelMeterConversion.cpp @@ -48,10 +48,8 @@ #include //! 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) +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); @@ -65,46 +63,38 @@ void vpPixelMeterConversion::convertLine(const vpCameraParameters &cam, 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) +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 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 + 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(vpMath::comb(p, r)) * - static_cast(vpMath::comb(q, t)) * - pow(xc, (int)(p - r)) * pow(yc, (int)(q - t)) * - moment_pixel[r][t]; + m[p][q] += static_cast(vpMath::comb(p, r)) * static_cast(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 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)); + 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 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) { diff --git a/modules/core/src/camera/vpXmlParserCamera.cpp b/modules/core/src/camera/vpXmlParserCamera.cpp index b355112946..81dcd97099 100644 --- a/modules/core/src/camera/vpXmlParserCamera.cpp +++ b/modules/core/src/camera/vpXmlParserCamera.cpp @@ -82,8 +82,8 @@ 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) + : vpXmlParser(), camera(), camera_name(), image_width(0), image_height(0), subsampling_width(0), + subsampling_height(0), full_width(0), full_height(0) { } /*! @@ -91,9 +91,8 @@ vpXmlParserCamera::vpXmlParserCamera() \param twinParser : parser object to copy */ vpXmlParserCamera::vpXmlParserCamera(vpXmlParserCamera &twinParser) - : vpXmlParser(twinParser), camera(twinParser.camera), - camera_name(twinParser.camera_name), image_width(0), image_height(0), - subsampling_width(0), subsampling_height(0), full_width(0), full_height(0) + : vpXmlParser(twinParser), camera(twinParser.camera), camera_name(twinParser.camera_name), image_width(0), + image_height(0), subsampling_width(0), subsampling_height(0), full_width(0), full_height(0) { this->image_width = twinParser.image_width; @@ -109,8 +108,7 @@ vpXmlParserCamera::vpXmlParserCamera(vpXmlParserCamera &twinParser) \param twinParser : parser object to copy \return a copy of the input. */ -vpXmlParserCamera &vpXmlParserCamera:: -operator=(const vpXmlParserCamera &twinParser) +vpXmlParserCamera &vpXmlParserCamera::operator=(const vpXmlParserCamera &twinParser) { this->camera = twinParser.camera; this->camera_name = twinParser.camera_name; @@ -137,11 +135,9 @@ operator=(const vpXmlParserCamera &twinParser) \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) +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; @@ -193,9 +189,8 @@ int main() ss_additional_info << "" << vpTime::getDateTime() << ""; vpXmlParserCamera p; - if (p.save(cam, "camera.xml", "myCamera", 320, 240, -ss_additional_info.str()) != vpXmlParserCamera::SEQUENCE_OK) { std::cout << -"Cannot save camera parameters" << std::endl; + if (p.save(cam, "camera.xml", "myCamera", 320, 240, ss_additional_info.str()) != vpXmlParserCamera::SEQUENCE_OK) { + std::cout << "Cannot save camera parameters" << std::endl; } } \endcode @@ -210,30 +205,24 @@ ss_additional_info.str()) != vpXmlParserCamera::SEQUENCE_OK) { std::cout << \endcode */ -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, +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, const std::string &additionalInfo) { xmlDocPtr doc; xmlNodePtr node; xmlNodePtr nodeCamera = NULL; - doc = xmlReadFile(filename.c_str(), NULL, - XML_PARSE_NOWARNING + XML_PARSE_NOERROR + - XML_PARSE_NOBLANKS); + 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 https://visp.inria.fr/download/ .\n" - " It can be read with the parse method of\n" - " the vpXmlParserCamera class."); + xmlNodePtr node_tmp = xmlNewComment((xmlChar *)"This file stores intrinsic camera parameters used\n" + " in the vpCameraParameters Class of ViSP available\n" + " at https://visp.inria.fr/download/ .\n" + " It can be read with the parse method of\n" + " the vpXmlParserCamera class."); xmlAddChild(node, node_tmp); } @@ -245,8 +234,7 @@ int vpXmlParserCamera::save(const vpCameraParameters &cam, this->camera = cam; - int nbCamera = - count(doc, node, cam_name, cam.get_projModel(), im_width, im_height); + 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 "<< @@ -276,20 +264,17 @@ int vpXmlParserCamera::save(const vpCameraParameters &cam, if (nodeAdditionalInfo == NULL) { // Create the additional information node - xmlNodePtr node_comment = - xmlNewComment((xmlChar *)"Additional information"); + xmlNodePtr node_comment = xmlNewComment((xmlChar *)"Additional information"); xmlAddChild(nodeCamera, node_comment); - nodeAdditionalInfo = - xmlNewNode(NULL, (xmlChar *)LABEL_XML_ADDITIONAL_INFO); + nodeAdditionalInfo = xmlNewNode(NULL, (xmlChar *)LABEL_XML_ADDITIONAL_INFO); xmlAddChild(nodeCamera, nodeAdditionalInfo); } if (nodeAdditionalInfo != NULL) { // Add the information in this specific node xmlNodePtr pNewNode = NULL; - xmlParseInNodeContext(nodeAdditionalInfo, additionalInfo.c_str(), - (int)additionalInfo.length(), 0, &pNewNode); + xmlParseInNodeContext(nodeAdditionalInfo, additionalInfo.c_str(), (int)additionalInfo.length(), 0, &pNewNode); if (pNewNode != NULL) { while (pNewNode != NULL) { xmlAddChild(nodeAdditionalInfo, xmlCopyNode(pNewNode, 1)); @@ -325,11 +310,10 @@ int vpXmlParserCamera::save(const vpCameraParameters &cam, \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) +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; @@ -359,9 +343,8 @@ int vpXmlParserCamera::read( } */ if (prop == CODE_XML_CAMERA) { - if (SEQUENCE_OK == this->read_camera(doc, node, cam_name, projModel, - im_width, im_height, - subsampl_width, subsampl_height)) + if (SEQUENCE_OK == + this->read_camera(doc, node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height)) nbCamera++; } else back = SEQUENCE_ERROR; @@ -369,12 +352,10 @@ int vpXmlParserCamera::read( if (nbCamera == 0) { back = SEQUENCE_ERROR; - vpCERROR << "No camera parameters is available" << std::endl - << "with your specifications" << std::endl; + 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 + vpCERROR << nbCamera << " sets of camera parameters are available" << std::endl << "with your specifications : " << std::endl << "precise your choice..." << std::endl; } @@ -400,11 +381,10 @@ int vpXmlParserCamera::read( \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) +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; @@ -431,9 +411,8 @@ int vpXmlParserCamera::count( } */ if (prop == CODE_XML_CAMERA) { - if (SEQUENCE_OK == this->read_camera(doc, node, cam_name, projModel, - im_width, im_height, - subsampl_width, subsampl_height)) + if (SEQUENCE_OK == + this->read_camera(doc, node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height)) nbCamera++; } } @@ -460,12 +439,9 @@ int vpXmlParserCamera::count( \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) +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; @@ -492,8 +468,7 @@ xmlNodePtr vpXmlParserCamera::find_camera(xmlDocPtr doc, xmlNodePtr node, */ if (prop == CODE_XML_CAMERA) { if (SEQUENCE_OK == - this->read_camera_header(doc, node, cam_name, im_width, im_height, - subsampl_width, subsampl_height)) + this->read_camera_header(doc, node, cam_name, im_width, im_height, subsampl_width, subsampl_height)) return node; } } @@ -550,11 +525,10 @@ xmlNodePtr vpXmlParserCamera::find_additional_info(xmlNodePtr node) \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) +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. */ @@ -583,8 +557,7 @@ int vpXmlParserCamera::read_camera( 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; + std::cout << "Found camera with name: \"" << camera_name_tmp << "\"" << std::endl; xmlFree(val_char); break; } @@ -643,24 +616,16 @@ int vpXmlParserCamera::read_camera( 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))); + 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))); + 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) && + (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 { @@ -694,12 +659,9 @@ int vpXmlParserCamera::read_camera( \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) +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. */ @@ -772,12 +734,10 @@ int vpXmlParserCamera::read_camera_header(xmlDocPtr doc, xmlNodePtr node, break; } } - if (!((cam_name == camera_name_tmp) && - (im_width == image_width_tmp || im_width == 0) && + 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))) { + (subsampl_height == subsampling_height_tmp || subsampl_height == 0))) { back = SEQUENCE_ERROR; } return back; @@ -793,9 +753,8 @@ int vpXmlParserCamera::read_camera_header(xmlDocPtr doc, xmlNodePtr node, \return error code. */ -vpXmlParserCamera::vpXmlCodeSequenceType -vpXmlParserCamera::read_camera_model(xmlDocPtr doc, xmlNodePtr node, - vpCameraParameters &cam_tmp) +vpXmlParserCamera::vpXmlCodeSequenceType vpXmlParserCamera::read_camera_model(xmlDocPtr doc, xmlNodePtr node, + vpCameraParameters &cam_tmp) { // counter of the number of read parameters int nb = 0; @@ -879,8 +838,7 @@ vpXmlParserCamera::read_camera_model(xmlDocPtr doc, xmlNodePtr node, } if (model_type == NULL) { - vpERROR_TRACE( - "projection model type doesn't match with any known model !"); + vpERROR_TRACE("projection model type doesn't match with any known model !"); return SEQUENCE_ERROR; } @@ -903,8 +861,7 @@ vpXmlParserCamera::read_camera_model(xmlDocPtr doc, xmlNodePtr node, } cam_tmp.initPersProjWithDistortion(px, py, u0, v0, kud, kdu); } else { - vpERROR_TRACE( - "projection model type doesn't match with any known model !"); + vpERROR_TRACE("projection model type doesn't match with any known model !"); xmlFree(model_type); return SEQUENCE_ERROR; @@ -931,10 +888,8 @@ vpXmlParserCamera::read_camera_model(xmlDocPtr doc, xmlNodePtr node, \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, +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; @@ -951,8 +906,7 @@ int vpXmlParserCamera::write(xmlNodePtr node, const std::string &cam_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()); + xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_CAMERA_NAME, (xmlChar *)cam_name.c_str()); } if (im_width != 0 || im_height != 0) { @@ -963,13 +917,11 @@ int vpXmlParserCamera::write(xmlNodePtr node, const std::string &cam_name, xmlAddChild(node_camera, node_tmp); sprintf(str, "%u", im_width); - xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_WIDTH, - (xmlChar *)str); + xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_WIDTH, (xmlChar *)str); // sprintf(str, "%u", im_height); - xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_HEIGHT, - (xmlChar *)str); + 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."); @@ -977,28 +929,21 @@ int vpXmlParserCamera::write(xmlNodePtr node, const std::string &cam_name, // sprintf(str, "%u", subsampl_width); - xmlNewTextChild(node_camera, NULL, - (xmlChar *)LABEL_XML_SUBSAMPLING_WIDTH, - (xmlChar *)str); + xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_SUBSAMPLING_WIDTH, (xmlChar *)str); // 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"); + 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); // sprintf(str, "%u", im_width * subsampl_width); - xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_FULL_WIDTH, - (xmlChar *)str); + xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_FULL_WIDTH, (xmlChar *)str); // sprintf(str, "%u", im_height * subsampl_height); - xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_FULL_HEIGHT, - (xmlChar *)str); + xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_FULL_HEIGHT, (xmlChar *)str); } } @@ -1035,31 +980,26 @@ int vpXmlParserCamera::write_camera(xmlNodePtr node_camera) xmlAddChild(node_model, node_tmp); //without_distortion - xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_MODEL_TYPE, - (xmlChar *)LABEL_XML_MODEL_WITHOUT_DISTORTION); + 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); // sprintf(str, "%.10f", camera.get_px()); - xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_PX, - (xmlChar *)str); + xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_PX, (xmlChar *)str); // sprintf(str, "%.10f", camera.get_py()); - xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_PY, - (xmlChar *)str); + xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_PY, (xmlChar *)str); node_tmp = xmlNewComment((xmlChar *)"Principal point"); xmlAddChild(node_model, node_tmp); // sprintf(str, "%.10f", camera.get_u0()); - xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_U0, - (xmlChar *)str); + xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_U0, (xmlChar *)str); // sprintf(str, "%.10f", camera.get_v0()); - xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_V0, - (xmlChar *)str); + xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_V0, (xmlChar *)str); } break; case vpCameraParameters::perspectiveProjWithDistortion: @@ -1071,46 +1011,37 @@ int vpXmlParserCamera::write_camera(xmlNodePtr node_camera) node_tmp = xmlNewComment((xmlChar *)"Projection model type"); xmlAddChild(node_model, node_tmp); //with_distortion - xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_MODEL_TYPE, - (xmlChar *)LABEL_XML_MODEL_WITH_DISTORTION); + 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); // sprintf(str, "%.10f", camera.get_px()); - xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_PX, - (xmlChar *)str); + xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_PX, (xmlChar *)str); // sprintf(str, "%.10f", camera.get_py()); - xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_PY, - (xmlChar *)str); + xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_PY, (xmlChar *)str); node_tmp = xmlNewComment((xmlChar *)"Principal point"); xmlAddChild(node_model, node_tmp); // sprintf(str, "%.10f", camera.get_u0()); - xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_U0, - (xmlChar *)str); + xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_U0, (xmlChar *)str); // sprintf(str, "%.10f", camera.get_v0()); - xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_V0, - (xmlChar *)str); + xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_V0, (xmlChar *)str); // - node_tmp = xmlNewComment( - (xmlChar *)"Undistorted to distorted distortion parameter"); + 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); + xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_KUD, (xmlChar *)str); // - node_tmp = xmlNewComment( - (xmlChar *)"Distorted to undistorted distortion parameter"); + 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); + xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_KDU, (xmlChar *)str); } break; } @@ -1125,8 +1056,7 @@ int vpXmlParserCamera::write_camera(xmlNodePtr node_camera) \return error code. */ -vpXmlParserCamera::vpXmlCodeSequenceType -vpXmlParserCamera::str2xmlcode(char *str, vpXmlCodeType &res) +vpXmlParserCamera::vpXmlCodeSequenceType vpXmlParserCamera::str2xmlcode(char *str, vpXmlCodeType &res) { vpXmlCodeType val_int = CODE_XML_BAD; vpXmlCodeSequenceType back = vpXmlParserCamera::SEQUENCE_OK; diff --git a/modules/core/src/display/vpColor.cpp b/modules/core/src/display/vpColor.cpp index fbd7b6f1b5..9a235d397e 100644 --- a/modules/core/src/display/vpColor.cpp +++ b/modules/core/src/display/vpColor.cpp @@ -43,8 +43,7 @@ // 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)) +#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. @@ -103,29 +102,27 @@ 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 +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}; +vpColor colors[6] = {vpColor::blue, vpColor::green, vpColor::red, vpColor::cyan, vpColor::orange, vpColor::purple}; /*! Compare two colors. diff --git a/modules/core/src/display/vpDisplay.cpp b/modules/core/src/display/vpDisplay.cpp index 6f0dcad159..d7df91e0b3 100644 --- a/modules/core/src/display/vpDisplay.cpp +++ b/modules/core/src/display/vpDisplay.cpp @@ -56,9 +56,8 @@ Default constructor. */ vpDisplay::vpDisplay() - : m_displayHasBeenInitialized(false), m_windowXPosition(0), - m_windowYPosition(0), m_width(0), m_height(0), m_title(), m_scale(1), - m_scaleType(SCALE_DEFAULT) + : m_displayHasBeenInitialized(false), m_windowXPosition(0), m_windowYPosition(0), m_width(0), m_height(0), m_title(), + m_scale(1), m_scaleType(SCALE_DEFAULT) { } @@ -66,11 +65,9 @@ vpDisplay::vpDisplay() Copy constructor. */ vpDisplay::vpDisplay(const vpDisplay &d) - : m_displayHasBeenInitialized(d.m_displayHasBeenInitialized), - m_windowXPosition(d.m_windowXPosition), - m_windowYPosition(d.m_windowYPosition), m_width(d.m_width), - m_height(d.m_height), m_title(d.m_title), m_scale(d.m_scale), - m_scaleType(d.m_scaleType) + : m_displayHasBeenInitialized(d.m_displayHasBeenInitialized), m_windowXPosition(d.m_windowXPosition), + m_windowYPosition(d.m_windowYPosition), m_width(d.m_width), m_height(d.m_height), m_title(d.m_title), + m_scale(d.m_scale), m_scaleType(d.m_scaleType) { } @@ -144,8 +141,7 @@ int main() } \endcode */ -void vpDisplay::getImage(const vpImage &Isrc, - vpImage &Idest) +void vpDisplay::getImage(const vpImage &Isrc, vpImage &Idest) { if (Isrc.display != NULL) { (Isrc.display)->getImage(Idest); @@ -238,10 +234,8 @@ void vpDisplay::setDownScalingFactor(unsigned int scale) if (!m_displayHasBeenInitialized) m_scale = scale; else { - std::cout - << "Warning: Cannot apply the down scaling factor " << scale - << " to the display window since the display is initialized yet..." - << std::endl; + std::cout << "Warning: Cannot apply the down scaling factor " << scale + << " to the display window since the display is initialized yet..." << std::endl; } } @@ -250,25 +244,20 @@ void vpDisplay::setDownScalingFactor(unsigned int scale) * to display the image given the resolution of the screen. \param width, * height : Image size. \return */ -unsigned int vpDisplay::computeAutoScale(unsigned int width, - unsigned int height) +unsigned int vpDisplay::computeAutoScale(unsigned int width, unsigned int height) { unsigned int screen_width, screen_height; getScreenSize(screen_width, screen_height); - double wscale = - (std::max)(1., ceil(2. * (double)width / (double)screen_width)); - double hscale = - (std::max)(1., ceil(2. * (double)height / (double)screen_height)); - unsigned int scale = (unsigned int)(std::max)( - 1u, (std::max)((unsigned int)wscale, (unsigned int)hscale)); + double wscale = (std::max)(1., ceil(2. * (double)width / (double)screen_width)); + double hscale = (std::max)(1., ceil(2. * (double)height / (double)screen_height)); + unsigned int scale = (unsigned int)(std::max)(1u, (std::max)((unsigned int)wscale, (unsigned int)hscale)); return scale; } /*! * Set the down scaling factor either in auto mode or set manually. */ -void vpDisplay::setScale(vpScaleType scaleType, unsigned int width, - unsigned int height) +void vpDisplay::setScale(vpScaleType scaleType, unsigned int width, unsigned int height) { switch (scaleType) { case vpDisplay::SCALE_AUTO: @@ -324,8 +313,11 @@ int main() #elif defined VISP_HAVE_GDI vpDisplayGDI d; #endif - d.setDownScalingFactor(vpDisplay::SCALE_4); // Display in a 160 by 120 -windows size d.init(I); vpDisplay::display(I); vpDisplay::flush(I); + // Display in a 160 by 120 windows size + d.setDownScalingFactor(vpDisplay::SCALE_4); + d.init(I); + vpDisplay::display(I); + vpDisplay::flush(I); vpDisplay::getClick(I); // wait for a click to quit } \endcode diff --git a/modules/core/src/display/vpDisplay_impl.h b/modules/core/src/display/vpDisplay_impl.h index 3878b93e07..db551428d4 100644 --- a/modules/core/src/display/vpDisplay_impl.h +++ b/modules/core/src/display/vpDisplay_impl.h @@ -17,10 +17,8 @@ template void vp_display_display(const vpImage &I) } template -void vp_display_display_arrow(const vpImage &I, const vpImagePoint &ip1, - const vpImagePoint &ip2, const vpColor &color, - unsigned int w, unsigned int h, - unsigned int thickness) +void vp_display_display_arrow(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, + const vpColor &color, unsigned int w, unsigned int h, unsigned int thickness) { if (I.display != NULL) { (I.display)->displayArrow(ip1, ip2, color, w, h, thickness); @@ -28,9 +26,8 @@ void vp_display_display_arrow(const vpImage &I, const vpImagePoint &ip1, } template -void vp_display_display_arrow(const vpImage &I, int i1, int j1, int i2, - int j2, const vpColor &color, unsigned int w, - unsigned int h, unsigned int thickness) +void vp_display_display_arrow(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, + unsigned int w, unsigned int h, unsigned int thickness) { if (I.display != NULL) { vpImagePoint ip1, ip2; @@ -43,10 +40,8 @@ void vp_display_display_arrow(const vpImage &I, int i1, int j1, int i2, } template -void vp_display_display_camera(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, double size, - const vpColor &color, unsigned int thickness) +void vp_display_display_camera(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + double size, const vpColor &color, unsigned int thickness) { // used by display double halfSize = size / 2.0; @@ -65,17 +60,15 @@ void vp_display_display_camera(const vpImage &I, for (int i = 0; i < 4; i++) { vpMeterPixelConversion::convertPoint(cam, pt[i].p[0], pt[i].p[1], ip_1); - vpMeterPixelConversion::convertPoint(cam, pt[(i + 1) % 4].p[0], - pt[(i + 1) % 4].p[1], ip); + vpMeterPixelConversion::convertPoint(cam, pt[(i + 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); } } template -void vp_display_display_char_string(const vpImage &I, - const vpImagePoint &ip, - const char *string, const vpColor &color) +void vp_display_display_char_string(const vpImage &I, const vpImagePoint &ip, const char *string, + const vpColor &color) { if (I.display != NULL) { (I.display)->displayCharString(ip, string, color); @@ -83,8 +76,7 @@ void vp_display_display_char_string(const vpImage &I, } template -void vp_display_display_char_string(const vpImage &I, int i, int j, - const char *string, const vpColor &color) +void vp_display_display_char_string(const vpImage &I, int i, int j, const char *string, const vpColor &color) { if (I.display != NULL) { vpImagePoint ip; @@ -96,10 +88,8 @@ void vp_display_display_char_string(const vpImage &I, int i, int j, } template -void vp_display_display_circle(const vpImage &I, - const vpImagePoint ¢er, - unsigned int radius, const vpColor &color, - bool fill, unsigned int thickness) +void vp_display_display_circle(const vpImage &I, const vpImagePoint ¢er, unsigned int radius, + const vpColor &color, bool fill, unsigned int thickness) { if (I.display != NULL) { (I.display)->displayCircle(center, radius, color, fill, thickness); @@ -107,8 +97,7 @@ void vp_display_display_circle(const vpImage &I, } template -void vp_display_display_circle(const vpImage &I, int i, int j, - unsigned int radius, const vpColor &color, +void vp_display_display_circle(const vpImage &I, int i, int j, unsigned int radius, const vpColor &color, bool fill, unsigned int thickness) { if (I.display != NULL) { @@ -121,8 +110,7 @@ void vp_display_display_circle(const vpImage &I, int i, int j, } template -void vp_display_display_cross(const vpImage &I, const vpImagePoint &ip, - unsigned int size, const vpColor &color, +void vp_display_display_cross(const vpImage &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness) { if (I.display != NULL) { @@ -131,8 +119,7 @@ void vp_display_display_cross(const vpImage &I, const vpImagePoint &ip, } template -void vp_display_display_cross(const vpImage &I, int i, int j, - unsigned int size, const vpColor &color, +void vp_display_display_cross(const vpImage &I, int i, int j, unsigned int size, const vpColor &color, unsigned int thickness) { if (I.display != NULL) { @@ -145,9 +132,7 @@ void vp_display_display_cross(const vpImage &I, int i, int j, } template -void vp_display_display_dot_line(const vpImage &I, - const vpImagePoint &ip1, - const vpImagePoint &ip2, +void vp_display_display_dot_line(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness) { if (I.display != NULL) { @@ -156,8 +141,7 @@ void vp_display_display_dot_line(const vpImage &I, } template -void vp_display_display_dot_line(const vpImage &I, int i1, int j1, - int i2, int j2, const vpColor &color, +void vp_display_display_dot_line(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness) { if (I.display != NULL) { @@ -171,13 +155,9 @@ void vp_display_display_dot_line(const vpImage &I, int i1, int j1, } template -void vp_display_display_ellipse(const vpImage &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) +void vp_display_display_ellipse(const vpImage &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) { if (I.display != NULL) { double j1, i1; @@ -194,8 +174,7 @@ void vp_display_display_ellipse(const vpImage &I, if (use_centered_moments) { if (std::fabs(mu11_p) > std::numeric_limits::epsilon()) { - double val_p = - sqrt(vpMath::sqr(mu20_p - mu02_p) + 4 * vpMath::sqr(mu11_p)); + 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); @@ -216,9 +195,7 @@ void vp_display_display_ellipse(const vpImage &I, // [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)))); + 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) @@ -258,11 +235,8 @@ void vp_display_display_ellipse(const vpImage &I, } template -void vp_display_display_frame(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, double size, - const vpColor &color, unsigned int thickness, - const vpImagePoint &offset) +void vp_display_display_frame(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + double size, const vpColor &color, unsigned int thickness, const vpImagePoint &offset) { // used by display vpPoint o(0.0, 0.0, 0.0); @@ -281,37 +255,30 @@ void vp_display_display_frame(const vpImage &I, 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); + 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); + 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); + 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); + 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); + 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); + vpDisplay::displayArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness); } } template -void vp_display_display_line(const vpImage &I, const vpImagePoint &ip1, - const vpImagePoint &ip2, const vpColor &color, - unsigned int thickness) +void vp_display_display_line(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, + const vpColor &color, unsigned int thickness) { if (I.display != NULL) { (I.display)->displayLine(ip1, ip2, color, thickness); @@ -319,8 +286,7 @@ void vp_display_display_line(const vpImage &I, const vpImagePoint &ip1, } template -void vp_display_display_line(const vpImage &I, int i1, int j1, int i2, - int j2, const vpColor &color, +void vp_display_display_line(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness) { if (I.display != NULL) { @@ -334,8 +300,8 @@ void vp_display_display_line(const vpImage &I, int i1, int j1, int i2, } template -void vp_display_display_point(const vpImage &I, const vpImagePoint &ip, - const vpColor &color, unsigned int thickness) +void vp_display_display_point(const vpImage &I, const vpImagePoint &ip, const vpColor &color, + unsigned int thickness) { if (I.display != NULL) { (I.display)->displayPoint(ip, color, thickness); @@ -343,8 +309,7 @@ void vp_display_display_point(const vpImage &I, const vpImagePoint &ip, } template -void vp_display_display_point(const vpImage &I, int i, int j, - const vpColor &color, unsigned int thickness) +void vp_display_display_point(const vpImage &I, int i, int j, const vpColor &color, unsigned int thickness) { if (I.display != NULL) { vpImagePoint ip; @@ -355,34 +320,26 @@ void vp_display_display_point(const vpImage &I, int i, int j, } template -void vp_display_display_polygon(const vpImage &I, - const std::vector &vip, - const vpColor &color, unsigned int thickness) +void vp_display_display_polygon(const vpImage &I, const std::vector &vip, const vpColor &color, + unsigned int thickness) { 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); + (I.display)->displayLine(vip[i], vip[(i + 1) % vip.size()], color, thickness); } } template -void vp_display_display_rectangle(const vpImage &I, - const vpImagePoint &topLeft, - unsigned int width, unsigned int height, - const vpColor &color, bool fill, - unsigned int thickness) +void vp_display_display_rectangle(const vpImage &I, const vpImagePoint &topLeft, unsigned int width, + unsigned int height, const vpColor &color, bool fill, unsigned int thickness) { if (I.display != NULL) { - (I.display)->displayRectangle(topLeft, width, height, color, fill, - thickness); + (I.display)->displayRectangle(topLeft, width, height, color, fill, thickness); } } template -void vp_display_display_rectangle(const vpImage &I, - const vpRect &rectangle, - const vpColor &color, bool fill, +void vp_display_display_rectangle(const vpImage &I, const vpRect &rectangle, const vpColor &color, bool fill, unsigned int thickness) { if (I.display != NULL) { @@ -391,11 +348,8 @@ void vp_display_display_rectangle(const vpImage &I, } template -void vp_display_display_rectangle(const vpImage &I, - const vpImagePoint ¢er, float angle, - unsigned int width, unsigned int height, - const vpColor &color, - unsigned int thickness) +void vp_display_display_rectangle(const vpImage &I, const vpImagePoint ¢er, float angle, unsigned int width, + unsigned int height, const vpColor &color, unsigned int thickness) { if (I.display != NULL) { double i = center.get_i(); @@ -422,40 +376,30 @@ void vp_display_display_rectangle(const vpImage &I, } template -void vp_display_display_rectangle(const vpImage &I, - const vpImagePoint &topLeft, - const vpImagePoint &bottomRight, - const vpColor &color, bool fill, - unsigned int thickness) +void vp_display_display_rectangle(const vpImage &I, const vpImagePoint &topLeft, const vpImagePoint &bottomRight, + const vpColor &color, bool fill, unsigned int thickness) { if (I.display != NULL) { - (I.display)->displayRectangle(topLeft, bottomRight, color, fill, - thickness); + (I.display)->displayRectangle(topLeft, bottomRight, color, fill, thickness); } } template -void vp_display_display_rectangle(const vpImage &I, int i, int j, - unsigned int width, unsigned int height, - const vpColor &color, bool fill, - unsigned int thickness) +void vp_display_display_rectangle(const vpImage &I, int i, int j, unsigned int width, unsigned int height, + const vpColor &color, bool fill, unsigned int thickness) { if (I.display != NULL) { vpImagePoint topLeft; topLeft.set_i(i); topLeft.set_j(j); - (I.display)->displayRectangle(topLeft, width, height, color, fill, - thickness); + (I.display)->displayRectangle(topLeft, width, height, color, fill, thickness); } } template -void vp_display_display_rectangle(const vpImage &I, unsigned int i, - unsigned int j, float angle, - unsigned int width, unsigned int height, - const vpColor &color, - unsigned int thickness) +void vp_display_display_rectangle(const vpImage &I, unsigned int i, unsigned int j, float angle, + unsigned int width, unsigned int height, const vpColor &color, unsigned int thickness) { if (I.display != NULL) { // A, B, C, D, corners of the rectangle clockwise @@ -478,8 +422,7 @@ void vp_display_display_rectangle(const vpImage &I, unsigned int i, } } -template -void vp_display_display_roi(const vpImage &I, const vpRect &roi) +template void vp_display_display_roi(const vpImage &I, const vpRect &roi) { double top = floor(roi.getTop()); double left = floor(roi.getLeft()); @@ -488,16 +431,12 @@ void vp_display_display_roi(const vpImage &I, const vpRect &roi) 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) { - throw(vpException(vpException::dimensionError, - "Region of interest outside of the image")); + if (top < 0 || top > iheight || left < 0 || left > iwidth || top + roiheight > iheight || left + roiwidth > iwidth) { + throw(vpException(vpException::dimensionError, "Region of interest outside of the image")); } if (I.display != NULL) { - (I.display)->displayImageROI(I, vpImagePoint(top, left), - (unsigned int)roiwidth, - (unsigned int)roiheight); + (I.display)->displayImageROI(I, vpImagePoint(top, left), (unsigned int)roiwidth, (unsigned int)roiheight); } } @@ -508,18 +447,14 @@ template void vp_display_flush(const vpImage &I) } } -template -void vp_display_flush_roi(const vpImage &I, const vpRect &roi) +template void vp_display_flush_roi(const vpImage &I, const vpRect &roi) { if (I.display != NULL) { - (I.display)->flushDisplayROI(roi.getTopLeft(), - (unsigned int)roi.getWidth(), - (unsigned int)roi.getHeight()); + (I.display)->flushDisplayROI(roi.getTopLeft(), (unsigned int)roi.getWidth(), (unsigned int)roi.getHeight()); } } -template -bool vp_display_get_click(const vpImage &I, bool blocking) +template bool vp_display_get_click(const vpImage &I, bool blocking) { if (I.display != NULL) { return (I.display)->getClick(blocking); @@ -527,9 +462,7 @@ bool vp_display_get_click(const vpImage &I, bool blocking) return false; } -template -bool vp_display_get_click(const vpImage &I, vpImagePoint &ip, - bool blocking) +template bool vp_display_get_click(const vpImage &I, vpImagePoint &ip, bool blocking) { if (I.display != NULL) { return (I.display)->getClick(ip, blocking); @@ -538,8 +471,7 @@ bool vp_display_get_click(const vpImage &I, vpImagePoint &ip, } template -bool vp_display_get_click(const vpImage &I, vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, +bool vp_display_get_click(const vpImage &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { if (I.display != NULL) { @@ -549,8 +481,7 @@ bool vp_display_get_click(const vpImage &I, vpImagePoint &ip, } template -bool vp_display_get_click_up(const vpImage &I, vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, +bool vp_display_get_click_up(const vpImage &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { if (I.display != NULL) { @@ -559,8 +490,7 @@ bool vp_display_get_click_up(const vpImage &I, vpImagePoint &ip, return false; } -template -bool vp_display_get_keyboard_event(const vpImage &I, bool blocking) +template bool vp_display_get_keyboard_event(const vpImage &I, bool blocking) { if (I.display != NULL) { return (I.display)->getKeyboardEvent(blocking); @@ -568,9 +498,7 @@ bool vp_display_get_keyboard_event(const vpImage &I, bool blocking) return false; } -template -bool vp_display_get_keyboard_event(const vpImage &I, std::string &key, - bool blocking) +template bool vp_display_get_keyboard_event(const vpImage &I, std::string &key, bool blocking) { if (I.display != NULL) { return (I.display)->getKeyboardEvent(key, blocking); @@ -578,9 +506,7 @@ bool vp_display_get_keyboard_event(const vpImage &I, std::string &key, return false; } -template -bool vp_display_get_keyboard_event(const vpImage &I, char *key, - bool blocking) +template bool vp_display_get_keyboard_event(const vpImage &I, char *key, bool blocking) { if (I.display != NULL) { std::string str; @@ -591,9 +517,7 @@ bool vp_display_get_keyboard_event(const vpImage &I, char *key, return false; } -template -bool vp_display_get_pointer_motion_event(const vpImage &I, - vpImagePoint &ip) +template bool vp_display_get_pointer_motion_event(const vpImage &I, vpImagePoint &ip) { if (I.display != NULL) { return (I.display)->getPointerMotionEvent(ip); @@ -601,8 +525,7 @@ bool vp_display_get_pointer_motion_event(const vpImage &I, return false; } -template -bool vp_display_get_pointer_position(const vpImage &I, vpImagePoint &ip) +template bool vp_display_get_pointer_position(const vpImage &I, vpImagePoint &ip) { if (I.display != NULL) { return (I.display)->getPointerPosition(ip); @@ -610,8 +533,7 @@ bool vp_display_get_pointer_position(const vpImage &I, vpImagePoint &ip) return false; } -template -void vp_display_set_background(const vpImage &I, const vpColor &color) +template void vp_display_set_background(const vpImage &I, const vpColor &color) { if (I.display != NULL) { (I.display)->clearDisplay(color); @@ -619,8 +541,7 @@ void vp_display_set_background(const vpImage &I, const vpColor &color) } template -void vp_display_display_text(const vpImage &I, const vpImagePoint &ip, - const std::string &s, const vpColor &color) +void vp_display_display_text(const vpImage &I, const vpImagePoint &ip, const std::string &s, const vpColor &color) { if (I.display != NULL) { (I.display)->displayCharString(ip, s.c_str(), color); @@ -628,8 +549,7 @@ void vp_display_display_text(const vpImage &I, const vpImagePoint &ip, } template -void vp_display_display_text(const vpImage &I, int i, int j, - const std::string &s, const vpColor &color) +void vp_display_display_text(const vpImage &I, int i, int j, const std::string &s, const vpColor &color) { if (I.display != NULL) { vpImagePoint ip; @@ -640,40 +560,32 @@ void vp_display_display_text(const vpImage &I, int i, int j, } } -template -void vp_display_set_font(const vpImage &I, const std::string &fontname) +template void vp_display_set_font(const vpImage &I, const std::string &fontname) { if (I.display != NULL) { (I.display)->setFont(fontname); } } -template -void vp_display_set_title(const vpImage &I, - const std::string &windowtitle) +template void vp_display_set_title(const vpImage &I, const std::string &windowtitle) { if (I.display != NULL) { (I.display)->setTitle(windowtitle); } } -template -void vp_display_set_window_position(const vpImage &I, int winx, - int winy) +template void vp_display_set_window_position(const vpImage &I, int winx, int winy) { if (I.display != NULL) { (I.display)->setWindowPosition(winx, winy); } } -template -unsigned int vp_display_get_down_scaling_factor(const vpImage &I) +template unsigned int vp_display_get_down_scaling_factor(const vpImage &I) { if (I.display != NULL) { return (I.display)->getDownScalingFactor(); } else { - throw(vpException( - vpException::fatalError, - "Cannot get the down scaling factor: Display is not initialized")); + throw(vpException(vpException::fatalError, "Cannot get the down scaling factor: Display is not initialized")); } } diff --git a/modules/core/src/display/vpDisplay_rgba.cpp b/modules/core/src/display/vpDisplay_rgba.cpp index 644d4df087..3c75d09c1f 100644 --- a/modules/core/src/display/vpDisplay_rgba.cpp +++ b/modules/core/src/display/vpDisplay_rgba.cpp @@ -20,10 +20,8 @@ void vpDisplay::close(vpImage &I) { vp_display_close(I); } \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 &I, - const vpImagePoint &ip1, const vpImagePoint &ip2, - const vpColor &color, unsigned int w, - unsigned int h, unsigned int thickness) +void vpDisplay::displayArrow(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, + const vpColor &color, unsigned int w, unsigned int h, unsigned int thickness) { vp_display_display_arrow(I, ip1, ip2, color, w, h, thickness); } @@ -38,9 +36,8 @@ void vpDisplay::displayArrow(const vpImage &I, \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 &I, int i1, int j1, int i2, - int j2, const vpColor &color, unsigned int w, - unsigned int h, unsigned int thickness) +void vpDisplay::displayArrow(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, + unsigned int w, unsigned int h, unsigned int thickness) { vp_display_display_arrow(I, i1, j1, i2, j2, color, w, h, thickness); } @@ -58,10 +55,8 @@ void vpDisplay::displayArrow(const vpImage &I, int i1, int j1, int i2, \param color : Color used to display the camera in the image. \param thickness : Thickness of the graphics drawing. */ -void vpDisplay::displayCamera(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, double size, - const vpColor &color, unsigned int thickness) +void vpDisplay::displayCamera(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + double size, const vpColor &color, unsigned int thickness) { vp_display_display_camera(I, cMo, cam, size, color, thickness); } @@ -79,8 +74,7 @@ void vpDisplay::displayCamera(const vpImage &I, \sa setFont(), displayText() */ -void vpDisplay::displayCharString(const vpImage &I, - const vpImagePoint &ip, const char *string, +void vpDisplay::displayCharString(const vpImage &I, const vpImagePoint &ip, const char *string, const vpColor &color) { vp_display_display_char_string(I, ip, string, color); @@ -99,8 +93,7 @@ void vpDisplay::displayCharString(const vpImage &I, \sa setFont(), displayText() */ -void vpDisplay::displayCharString(const vpImage &I, int i, int j, - const char *string, const vpColor &color) +void vpDisplay::displayCharString(const vpImage &I, int i, int j, const char *string, const vpColor &color) { vp_display_display_char_string(I, i, j, string, color); } @@ -115,10 +108,8 @@ void vpDisplay::displayCharString(const vpImage &I, int i, int j, \param thickness : Thickness of the circle. This parameter is only useful when \e fill is set to false. */ -void vpDisplay::displayCircle(const vpImage &I, - const vpImagePoint ¢er, unsigned int radius, - const vpColor &color, bool fill, - unsigned int thickness) +void vpDisplay::displayCircle(const vpImage &I, const vpImagePoint ¢er, unsigned int radius, + const vpColor &color, bool fill, unsigned int thickness) { vp_display_display_circle(I, center, radius, color, fill, thickness); } @@ -133,8 +124,7 @@ void vpDisplay::displayCircle(const vpImage &I, \param thickness : Thickness of the circle. This parameter is only useful when \e fill is set to false. */ -void vpDisplay::displayCircle(const vpImage &I, int i, int j, - unsigned int radius, const vpColor &color, +void vpDisplay::displayCircle(const vpImage &I, int i, int j, unsigned int radius, const vpColor &color, bool fill, unsigned int thickness) { vp_display_display_circle(I, i, j, radius, color, fill, thickness); @@ -148,8 +138,7 @@ void vpDisplay::displayCircle(const vpImage &I, int i, int j, \param color : Cross color. \param thickness : Thickness of the lines used to display the cross. */ -void vpDisplay::displayCross(const vpImage &I, const vpImagePoint &ip, - unsigned int size, const vpColor &color, +void vpDisplay::displayCross(const vpImage &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness) { vp_display_display_cross(I, ip, size, color, thickness); @@ -163,8 +152,7 @@ void vpDisplay::displayCross(const vpImage &I, const vpImagePoint &ip, \param color : Cross color. \param thickness : Thickness of the lines used to display the cross. */ -void vpDisplay::displayCross(const vpImage &I, int i, int j, - unsigned int size, const vpColor &color, +void vpDisplay::displayCross(const vpImage &I, int i, int j, unsigned int size, const vpColor &color, unsigned int thickness) { vp_display_display_cross(I, i, j, size, color, thickness); @@ -177,10 +165,8 @@ void vpDisplay::displayCross(const vpImage &I, int i, int j, \param color : Line color. \param thickness : Dashed line thickness. */ -void vpDisplay::displayDotLine(const vpImage &I, - const vpImagePoint &ip1, - const vpImagePoint &ip2, const vpColor &color, - unsigned int thickness) +void vpDisplay::displayDotLine(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, + const vpColor &color, unsigned int thickness) { vp_display_display_dot_line(I, ip1, ip2, color, thickness); } @@ -193,8 +179,7 @@ void vpDisplay::displayDotLine(const vpImage &I, \param color : Line color. \param thickness : Dashed line thickness. */ -void vpDisplay::displayDotLine(const vpImage &I, int i1, int j1, - int i2, int j2, const vpColor &color, +void vpDisplay::displayDotLine(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness) { vp_display_display_dot_line(I, i1, j1, i2, j2, color, thickness); @@ -208,10 +193,8 @@ void vpDisplay::displayDotLine(const vpImage &I, int i1, int j1, last image points. \param color : Line color. \param thickness : Dashed line thickness. */ -void vpDisplay::displayDotLine(const vpImage &I, - const std::vector &ips, - const bool closeTheShape, const vpColor &color, - unsigned int thickness) +void vpDisplay::displayDotLine(const vpImage &I, const std::vector &ips, const bool closeTheShape, + const vpColor &color, unsigned int thickness) { if (ips.size() <= 1) return; @@ -259,24 +242,23 @@ void vpDisplay::displayDotLine(const vpImage &I, 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; + 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); + ellipse.get_mu11(), ellipse.get_mu02(), true, vpColor::orange, 1); vpDisplay::flush(I); \endcode */ -void vpDisplay::displayEllipse(const vpImage &I, - const vpImagePoint ¢er, - const double &coef1, const double &coef2, - const double &coef3, bool use_centered_moments, +void vpDisplay::displayEllipse(const vpImage &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, + vpDisplay::displayEllipse(I, center, coef1, coef2, coef3, 0., vpMath::rad(360), use_centered_moments, color, thickness); } @@ -319,7 +301,9 @@ void vpDisplay::displayEllipse(const vpImage &I, 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; + the result of a tracking. + \code + vpMeEllipse ellipse; ... vpDisplay::display(I); ellipse.track(I); @@ -327,19 +311,15 @@ void vpDisplay::displayEllipse(const vpImage &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); + ellipse.getHighestAngle(), true, vpColor::orange, 1); + vpDisplay::flush(I); \endcode */ -void vpDisplay::displayEllipse(const vpImage &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) +void vpDisplay::displayEllipse(const vpImage &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) { - vp_display_display_ellipse(I, center, coef1, coef2, coef3, theta1, theta2, - use_centered_moments, color, thickness); + vp_display_display_ellipse(I, center, coef1, coef2, coef3, theta1, theta2, use_centered_moments, color, thickness); } /*! @@ -357,11 +337,8 @@ void vpDisplay::displayEllipse(const vpImage &I, \param offset : Offset in pixels applied to the frame origin location in the image. */ -void vpDisplay::displayFrame(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, double size, - const vpColor &color, unsigned int thickness, - const vpImagePoint &offset) +void vpDisplay::displayFrame(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + double size, const vpColor &color, unsigned int thickness, const vpImagePoint &offset) { vp_display_display_frame(I, cMo, cam, size, color, thickness, offset); } @@ -373,9 +350,8 @@ void vpDisplay::displayFrame(const vpImage &I, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplay::displayLine(const vpImage &I, const vpImagePoint &ip1, - const vpImagePoint &ip2, const vpColor &color, - unsigned int thickness) +void vpDisplay::displayLine(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, + const vpColor &color, unsigned int thickness) { vp_display_display_line(I, ip1, ip2, color, thickness); } @@ -388,8 +364,7 @@ void vpDisplay::displayLine(const vpImage &I, const vpImagePoint &ip1, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int i2, - int j2, const vpColor &color, +void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness) { vp_display_display_line(I, i1, j1, i2, j2, color, thickness); @@ -402,10 +377,8 @@ void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int i2, \param closeTheShape : If true, draw a line from the first and last image points. \param color : Line color. \param thickness : Line thickness. */ -void vpDisplay::displayLine(const vpImage &I, - const std::vector &ips, - const bool closeTheShape, const vpColor &color, - unsigned int thickness) +void vpDisplay::displayLine(const vpImage &I, const std::vector &ips, const bool closeTheShape, + const vpColor &color, unsigned int thickness) { if (ips.size() <= 1) return; @@ -424,8 +397,8 @@ void vpDisplay::displayLine(const vpImage &I, \param color : Point color. \param thickness : Thickness of the point */ -void vpDisplay::displayPoint(const vpImage &I, const vpImagePoint &ip, - const vpColor &color, unsigned int thickness) +void vpDisplay::displayPoint(const vpImage &I, const vpImagePoint &ip, const vpColor &color, + unsigned int thickness) { vp_display_display_point(I, ip, color, thickness); } @@ -437,8 +410,7 @@ void vpDisplay::displayPoint(const vpImage &I, const vpImagePoint &ip, \param color : Point color. \param thickness : Thickness of the point */ -void vpDisplay::displayPoint(const vpImage &I, int i, int j, - const vpColor &color, unsigned int thickness) +void vpDisplay::displayPoint(const vpImage &I, int i, int j, const vpColor &color, unsigned int thickness) { vp_display_display_point(I, i, j, color, thickness); } @@ -450,9 +422,8 @@ void vpDisplay::displayPoint(const vpImage &I, int i, int j, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplay::displayPolygon(const vpImage &I, - const std::vector &vip, - const vpColor &color, unsigned int thickness) +void vpDisplay::displayPolygon(const vpImage &I, const std::vector &vip, const vpColor &color, + unsigned int thickness) { vp_display_display_polygon(I, vip, color, thickness); } @@ -471,14 +442,10 @@ void vpDisplay::displayPolygon(const vpImage &I, rectangle. This parameter is only useful when \e fill is set to false. */ -void vpDisplay::displayRectangle(const vpImage &I, - const vpImagePoint &topLeft, - unsigned int width, unsigned int height, - const vpColor &color, bool fill, - unsigned int thickness) +void vpDisplay::displayRectangle(const vpImage &I, const vpImagePoint &topLeft, unsigned int width, + unsigned int height, const vpColor &color, bool fill, unsigned int thickness) { - vp_display_display_rectangle(I, topLeft, width, height, color, fill, - thickness); + vp_display_display_rectangle(I, topLeft, width, height, color, fill, thickness); } /*! @@ -495,13 +462,10 @@ void vpDisplay::displayRectangle(const vpImage &I, rectangle. This parameter is only useful when \e fill is set to false. */ -void vpDisplay::displayRectangle(const vpImage &I, int i, int j, - unsigned int width, unsigned int height, - const vpColor &color, bool fill, - unsigned int thickness) +void vpDisplay::displayRectangle(const vpImage &I, int i, int j, unsigned int width, unsigned int height, + const vpColor &color, bool fill, unsigned int thickness) { - vp_display_display_rectangle(I, i, j, width, height, color, fill, - thickness); + vp_display_display_rectangle(I, i, j, width, height, color, fill, thickness); } /*! @@ -517,9 +481,7 @@ void vpDisplay::displayRectangle(const vpImage &I, int i, int j, rectangle. This parameter is only useful when \e fill is set to false. */ -void vpDisplay::displayRectangle(const vpImage &I, - const vpRect &rectangle, - const vpColor &color, bool fill, +void vpDisplay::displayRectangle(const vpImage &I, const vpRect &rectangle, const vpColor &color, bool fill, unsigned int thickness) { vp_display_display_rectangle(I, rectangle, color, fill, thickness); @@ -538,13 +500,10 @@ void vpDisplay::displayRectangle(const vpImage &I, \param thickness : Thickness of the four lines used to display the rectangle. */ -void vpDisplay::displayRectangle(const vpImage &I, - const vpImagePoint ¢er, float angle, - unsigned int width, unsigned int height, - const vpColor &color, unsigned int thickness) +void vpDisplay::displayRectangle(const vpImage &I, const vpImagePoint ¢er, float angle, unsigned int width, + unsigned int height, const vpColor &color, unsigned int thickness) { - vp_display_display_rectangle(I, center, angle, width, height, color, - thickness); + vp_display_display_rectangle(I, center, angle, width, height, color, thickness); } /*! @@ -561,14 +520,10 @@ void vpDisplay::displayRectangle(const vpImage &I, rectangle. This parameter is only useful when \e fill is set to false. */ -void vpDisplay::displayRectangle(const vpImage &I, - const vpImagePoint &topLeft, - const vpImagePoint &bottomRight, - const vpColor &color, bool fill, - unsigned int thickness) +void vpDisplay::displayRectangle(const vpImage &I, const vpImagePoint &topLeft, const vpImagePoint &bottomRight, + const vpColor &color, bool fill, unsigned int thickness) { - vp_display_display_rectangle(I, topLeft, bottomRight, color, fill, - thickness); + vp_display_display_rectangle(I, topLeft, bottomRight, color, fill, thickness); } /*! @@ -584,13 +539,10 @@ void vpDisplay::displayRectangle(const vpImage &I, \param thickness : Thickness of the four lines used to display the rectangle. */ -void vpDisplay::displayRectangle(const vpImage &I, unsigned int i, - unsigned int j, float angle, - unsigned int width, unsigned int height, - const vpColor &color, unsigned int thickness) +void vpDisplay::displayRectangle(const vpImage &I, unsigned int i, unsigned int j, float angle, + unsigned int width, unsigned int height, const vpColor &color, unsigned int thickness) { - vp_display_display_rectangle(I, i, j, angle, width, height, color, - thickness); + vp_display_display_rectangle(I, i, j, angle, width, height, color, thickness); } /*! @@ -605,8 +557,8 @@ void vpDisplay::displayRectangle(const vpImage &I, unsigned int i, \sa setFont() */ -void vpDisplay::displayText(const vpImage &I, const vpImagePoint &ip, - const std::string &s, const vpColor &color) +void vpDisplay::displayText(const vpImage &I, const vpImagePoint &ip, const std::string &s, + const vpColor &color) { vp_display_display_text(I, ip, s, color); } @@ -623,8 +575,7 @@ void vpDisplay::displayText(const vpImage &I, const vpImagePoint &ip, \sa setFont() */ -void vpDisplay::displayText(const vpImage &I, int i, int j, - const std::string &s, const vpColor &color) +void vpDisplay::displayText(const vpImage &I, int i, int j, const std::string &s, const vpColor &color) { vp_display_display_text(I, i, j, s, color); } @@ -669,10 +620,7 @@ void vpDisplay::flush(const vpImage &I) { vp_display_flush(I); } \sa flush() */ -void vpDisplay::flushROI(const vpImage &I, const vpRect &roi) -{ - vp_display_flush_roi(I, roi); -} +void vpDisplay::flushROI(const vpImage &I, const vpRect &roi) { vp_display_flush_roi(I, roi); } /*! Display image \e I. @@ -691,10 +639,7 @@ void vpDisplay::display(const vpImage &I) { vp_display_display(I); } Update the display with the content of the image that is in the region of interest. \param I : Image. \param roi : Region of interest. */ -void vpDisplay::displayROI(const vpImage &I, const vpRect &roi) -{ - vp_display_display_roi(I, roi); -} +void vpDisplay::displayROI(const vpImage &I, const vpRect &roi) { vp_display_display_roi(I, roi); } /*! Wait for a click from one of the mouse button. @@ -713,10 +658,7 @@ void vpDisplay::displayROI(const vpImage &I, const vpRect &roi) - false if no button was clicked. This can occur if blocking is set to \e false. */ -bool vpDisplay::getClick(const vpImage &I, bool blocking) -{ - return vp_display_get_click(I, blocking); -} +bool vpDisplay::getClick(const vpImage &I, bool blocking) { return vp_display_get_click(I, blocking); } /*! Wait for a click from one of the mouse button and get the position @@ -736,8 +678,7 @@ bool vpDisplay::getClick(const vpImage &I, bool blocking) button is pressed, the location of the mouse pointer is updated in \e ip. */ -bool vpDisplay::getClick(const vpImage &I, vpImagePoint &ip, - bool blocking) +bool vpDisplay::getClick(const vpImage &I, vpImagePoint &ip, bool blocking) { return vp_display_get_click(I, ip, blocking); } @@ -762,8 +703,7 @@ bool vpDisplay::getClick(const vpImage &I, vpImagePoint &ip, button is pressed, the location of the mouse pointer is updated in \e ip. */ -bool vpDisplay::getClick(const vpImage &I, vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, +bool vpDisplay::getClick(const vpImage &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { return vp_display_get_click(I, ip, button, blocking); @@ -785,9 +725,7 @@ bool vpDisplay::getClick(const vpImage &I, vpImagePoint &ip, \return true if a mouse button is pressed, false otherwise. */ -bool vpDisplay::getClick(const vpImage &I, - vpMouseButton::vpMouseButtonType &button, - bool blocking) +bool vpDisplay::getClick(const vpImage &I, vpMouseButton::vpMouseButtonType &button, bool blocking) { vpImagePoint ip; return vpDisplay::getClick(I, ip, button, blocking); @@ -813,8 +751,7 @@ bool vpDisplay::getClick(const vpImage &I, button is released, the location of the mouse pointer is updated in \e ip. */ -bool vpDisplay::getClickUp(const vpImage &I, vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, +bool vpDisplay::getClickUp(const vpImage &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { return vp_display_get_click_up(I, ip, button, blocking); @@ -836,9 +773,7 @@ bool vpDisplay::getClickUp(const vpImage &I, vpImagePoint &ip, \return true if a mouse button is released, false otherwise. */ -bool vpDisplay::getClickUp(const vpImage &I, - vpMouseButton::vpMouseButtonType &button, - bool blocking) +bool vpDisplay::getClickUp(const vpImage &I, vpMouseButton::vpMouseButtonType &button, bool blocking) { vpImagePoint ip; return vpDisplay::getClickUp(I, ip, button, blocking); @@ -910,9 +845,11 @@ int main() // Non blocking keyboard event loop int cpt_event = 0; - std::cout << "Enter a non blocking keyboard event detection loop..." << -std::endl; do { bool event = vpDisplay::getKeyboardEvent(I, false); if (event) -{ std::cout << "A keyboard event was detected" << std::endl; cpt_event ++; + std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; + do { + bool event = vpDisplay::getKeyboardEvent(I, false); + if (event) { + std::cout << "A keyboard event was detected" << std::endl; cpt_event ++; } vpTime::wait(5); // wait 5 ms @@ -998,9 +935,12 @@ int main() // Non blocking keyboard event loop int cpt_event = 0; std::string key; - std::cout << "Enter a non blocking keyboard event detection loop..." << -std::endl; do { bool event = vpDisplay::getKeyboardEvent(I, key, false); if -(event) { std::cout << "Key detected: " << key << std::endl; cpt_event ++; + std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; + do { + bool event = vpDisplay::getKeyboardEvent(I, key, false); + if (event) { + std::cout << "Key detected: " << key << std::endl; + cpt_event ++; } vpTime::wait(5); // wait 5 ms @@ -1012,8 +952,7 @@ std::endl; do { bool event = vpDisplay::getKeyboardEvent(I, key, false); if } \endcode */ -bool vpDisplay::getKeyboardEvent(const vpImage &I, std::string &key, - bool blocking) +bool vpDisplay::getKeyboardEvent(const vpImage &I, std::string &key, bool blocking) { return vp_display_get_keyboard_event(I, key, blocking); } @@ -1087,8 +1026,9 @@ int main() // Non blocking keyboard event loop int cpt_event = 0; char key[10]; - std::cout << "Enter a non blocking keyboard event detection loop..." << -std::endl; do { bool event = vpDisplay::getKeyboardEvent(I, &key[Ø], false); + std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; + do { + bool event = vpDisplay::getKeyboardEvent(I, &key[Ø], false); if (event) { std::cout << "Key detected: " << key << std::endl; cpt_event ++; @@ -1103,8 +1043,7 @@ std::endl; do { bool event = vpDisplay::getKeyboardEvent(I, &key[Ø], false); } \endcode */ -bool vpDisplay::getKeyboardEvent(const vpImage &I, char *key, - bool blocking) +bool vpDisplay::getKeyboardEvent(const vpImage &I, char *key, bool blocking) { return vp_display_get_keyboard_event(I, key, blocking); } @@ -1118,8 +1057,7 @@ bool vpDisplay::getKeyboardEvent(const vpImage &I, char *key, \return true if a pointer motion event was received, false otherwise. */ -bool vpDisplay::getPointerMotionEvent(const vpImage &I, - vpImagePoint &ip) +bool vpDisplay::getPointerMotionEvent(const vpImage &I, vpImagePoint &ip) { return vp_display_get_pointer_motion_event(I, ip); } @@ -1147,10 +1085,7 @@ bool vpDisplay::getPointerPosition(const vpImage &I, vpImagePoint &ip) \exception vpDisplayException::notInitializedError : If the video device is not initialized. */ -void vpDisplay::setBackground(const vpImage &I, const vpColor &color) -{ - vp_display_set_background(I, color); -} +void vpDisplay::setBackground(const vpImage &I, const vpColor &color) { vp_display_set_background(I, color); } /*! Set the font of a text printed in the display overlay. To print a @@ -1165,10 +1100,7 @@ void vpDisplay::setBackground(const vpImage &I, const vpColor &color) \sa displayText() */ -void vpDisplay::setFont(const vpImage &I, const std::string &fontname) -{ - vp_display_set_font(I, fontname); -} +void vpDisplay::setFont(const vpImage &I, const std::string &fontname) { vp_display_set_font(I, fontname); } /*! Set the windows title. @@ -1177,8 +1109,7 @@ void vpDisplay::setFont(const vpImage &I, const std::string &fontname) \param I : Image associated to the display window. \param windowtitle : Window title. */ -void vpDisplay::setTitle(const vpImage &I, - const std::string &windowtitle) +void vpDisplay::setTitle(const vpImage &I, const std::string &windowtitle) { vp_display_set_title(I, windowtitle); } @@ -1193,8 +1124,7 @@ void vpDisplay::setTitle(const vpImage &I, \exception vpDisplayException::notInitializedError : If the video device is not initialized. */ -void vpDisplay::setWindowPosition(const vpImage &I, int winx, - int winy) +void vpDisplay::setWindowPosition(const vpImage &I, int winx, int winy) { vp_display_set_window_position(I, winx, winy); } @@ -1208,7 +1138,4 @@ void vpDisplay::setWindowPosition(const vpImage &I, int winx, \exception vpDisplayException::notInitializedError : If the video device is not initialized. */ -unsigned int vpDisplay::getDownScalingFactor(const vpImage &I) -{ - return vp_display_get_down_scaling_factor(I); -} +unsigned int vpDisplay::getDownScalingFactor(const vpImage &I) { return vp_display_get_down_scaling_factor(I); } diff --git a/modules/core/src/display/vpDisplay_uchar.cpp b/modules/core/src/display/vpDisplay_uchar.cpp index 90e8012db6..a55a2a9d6a 100644 --- a/modules/core/src/display/vpDisplay_uchar.cpp +++ b/modules/core/src/display/vpDisplay_uchar.cpp @@ -20,10 +20,8 @@ void vpDisplay::close(vpImage &I) { vp_display_close(I); } \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 &I, - const vpImagePoint &ip1, const vpImagePoint &ip2, - const vpColor &color, unsigned int w, - unsigned int h, unsigned int thickness) +void vpDisplay::displayArrow(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, + const vpColor &color, unsigned int w, unsigned int h, unsigned int thickness) { vp_display_display_arrow(I, ip1, ip2, color, w, h, thickness); } @@ -38,10 +36,8 @@ void vpDisplay::displayArrow(const vpImage &I, \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 &I, int i1, int j1, - int i2, int j2, const vpColor &color, - unsigned int w, unsigned int h, - unsigned int thickness) +void vpDisplay::displayArrow(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, + unsigned int w, unsigned int h, unsigned int thickness) { vp_display_display_arrow(I, i1, j1, i2, j2, color, w, h, thickness); } @@ -59,10 +55,8 @@ void vpDisplay::displayArrow(const vpImage &I, int i1, int j1, \param color : Color used to display the camera in the image. \param thickness : Thickness of the graphics drawing. */ -void vpDisplay::displayCamera(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, double size, - const vpColor &color, unsigned int thickness) +void vpDisplay::displayCamera(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) { vp_display_display_camera(I, cMo, cam, size, color, thickness); } @@ -80,8 +74,7 @@ void vpDisplay::displayCamera(const vpImage &I, \sa setFont(), displayText() */ -void vpDisplay::displayCharString(const vpImage &I, - const vpImagePoint &ip, const char *string, +void vpDisplay::displayCharString(const vpImage &I, const vpImagePoint &ip, const char *string, const vpColor &color) { vp_display_display_char_string(I, ip, string, color); @@ -100,8 +93,7 @@ void vpDisplay::displayCharString(const vpImage &I, \sa setFont(), displayText() */ -void vpDisplay::displayCharString(const vpImage &I, int i, - int j, const char *string, +void vpDisplay::displayCharString(const vpImage &I, int i, int j, const char *string, const vpColor &color) { vp_display_display_char_string(I, i, j, string, color); @@ -117,10 +109,8 @@ void vpDisplay::displayCharString(const vpImage &I, int i, \param thickness : Thickness of the circle. This parameter is only useful when \e fill is set to false. */ -void vpDisplay::displayCircle(const vpImage &I, - const vpImagePoint ¢er, unsigned int radius, - const vpColor &color, bool fill, - unsigned int thickness) +void vpDisplay::displayCircle(const vpImage &I, const vpImagePoint ¢er, unsigned int radius, + const vpColor &color, bool fill, unsigned int thickness) { vp_display_display_circle(I, center, radius, color, fill, thickness); } @@ -135,8 +125,7 @@ void vpDisplay::displayCircle(const vpImage &I, \param thickness : Thickness of the circle. This parameter is only useful when \e fill is set to false. */ -void vpDisplay::displayCircle(const vpImage &I, int i, int j, - unsigned int radius, const vpColor &color, +void vpDisplay::displayCircle(const vpImage &I, int i, int j, unsigned int radius, const vpColor &color, bool fill, unsigned int thickness) { vp_display_display_circle(I, i, j, radius, color, fill, thickness); @@ -150,8 +139,7 @@ void vpDisplay::displayCircle(const vpImage &I, int i, int j, \param color : Cross color. \param thickness : Thickness of the lines used to display the cross. */ -void vpDisplay::displayCross(const vpImage &I, - const vpImagePoint &ip, unsigned int size, +void vpDisplay::displayCross(const vpImage &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness) { vp_display_display_cross(I, ip, size, color, thickness); @@ -165,8 +153,7 @@ void vpDisplay::displayCross(const vpImage &I, \param color : Cross color. \param thickness : Thickness of the lines used to display the cross. */ -void vpDisplay::displayCross(const vpImage &I, int i, int j, - unsigned int size, const vpColor &color, +void vpDisplay::displayCross(const vpImage &I, int i, int j, unsigned int size, const vpColor &color, unsigned int thickness) { vp_display_display_cross(I, i, j, size, color, thickness); @@ -179,10 +166,8 @@ void vpDisplay::displayCross(const vpImage &I, int i, int j, \param color : Line color. \param thickness : Dashed line thickness. */ -void vpDisplay::displayDotLine(const vpImage &I, - const vpImagePoint &ip1, - const vpImagePoint &ip2, const vpColor &color, - unsigned int thickness) +void vpDisplay::displayDotLine(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, + const vpColor &color, unsigned int thickness) { vp_display_display_dot_line(I, ip1, ip2, color, thickness); } @@ -195,8 +180,7 @@ void vpDisplay::displayDotLine(const vpImage &I, \param color : Line color. \param thickness : Dashed line thickness. */ -void vpDisplay::displayDotLine(const vpImage &I, int i1, - int j1, int i2, int j2, const vpColor &color, +void vpDisplay::displayDotLine(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness) { vp_display_display_dot_line(I, i1, j1, i2, j2, color, thickness); @@ -210,10 +194,8 @@ void vpDisplay::displayDotLine(const vpImage &I, int i1, last image points. \param color : Line color. \param thickness : Dashed line thickness. */ -void vpDisplay::displayDotLine(const vpImage &I, - const std::vector &ips, - const bool closeTheShape, const vpColor &color, - unsigned int thickness) +void vpDisplay::displayDotLine(const vpImage &I, const std::vector &ips, + const bool closeTheShape, const vpColor &color, unsigned int thickness) { if (ips.size() <= 1) return; @@ -261,24 +243,23 @@ void vpDisplay::displayDotLine(const vpImage &I, 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; + 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); + ellipse.get_mu11(), ellipse.get_mu02(), true, vpColor::orange, 1); vpDisplay::flush(I); \endcode */ -void vpDisplay::displayEllipse(const vpImage &I, - const vpImagePoint ¢er, - const double &coef1, const double &coef2, - const double &coef3, bool use_centered_moments, +void vpDisplay::displayEllipse(const vpImage &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, + vpDisplay::displayEllipse(I, center, coef1, coef2, coef3, 0., vpMath::rad(360), use_centered_moments, color, thickness); } @@ -321,7 +302,9 @@ void vpDisplay::displayEllipse(const vpImage &I, 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; + the result of a tracking. + \code + vpMeEllipse ellipse; ... vpDisplay::display(I); ellipse.track(I); @@ -329,19 +312,14 @@ void vpDisplay::displayEllipse(const vpImage &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); + ellipse.getHighestAngle(), true, vpColor::orange, 1); vpDisplay::flush(I); \endcode */ -void vpDisplay::displayEllipse(const vpImage &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) +void vpDisplay::displayEllipse(const vpImage &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) { - vp_display_display_ellipse(I, center, coef1, coef2, coef3, theta1, theta2, - use_centered_moments, color, thickness); + vp_display_display_ellipse(I, center, coef1, coef2, coef3, theta1, theta2, use_centered_moments, color, thickness); } /*! @@ -359,10 +337,8 @@ void vpDisplay::displayEllipse(const vpImage &I, \param offset : Offset in pixels applied to the frame origin location in the image. */ -void vpDisplay::displayFrame(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, double size, - const vpColor &color, unsigned int thickness, +void vpDisplay::displayFrame(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness, const vpImagePoint &offset) { vp_display_display_frame(I, cMo, cam, size, color, thickness, offset); @@ -375,8 +351,7 @@ void vpDisplay::displayFrame(const vpImage &I, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplay::displayLine(const vpImage &I, - const vpImagePoint &ip1, const vpImagePoint &ip2, +void vpDisplay::displayLine(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness) { vp_display_display_line(I, ip1, ip2, color, thickness); @@ -390,8 +365,7 @@ void vpDisplay::displayLine(const vpImage &I, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplay::displayLine(const vpImage &I, int i1, int j1, - int i2, int j2, const vpColor &color, +void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness) { vp_display_display_line(I, i1, j1, i2, j2, color, thickness); @@ -404,10 +378,8 @@ void vpDisplay::displayLine(const vpImage &I, int i1, int j1, \param closeTheShape : If true, display a line from the first and last image points. \param color : Line color. \param thickness : Line thickness. */ -void vpDisplay::displayLine(const vpImage &I, - const std::vector &ips, - const bool closeTheShape, const vpColor &color, - unsigned int thickness) +void vpDisplay::displayLine(const vpImage &I, const std::vector &ips, + const bool closeTheShape, const vpColor &color, unsigned int thickness) { if (ips.size() <= 1) return; @@ -426,8 +398,7 @@ void vpDisplay::displayLine(const vpImage &I, \param color : Point color. \param thickness : Thickness of the point */ -void vpDisplay::displayPoint(const vpImage &I, - const vpImagePoint &ip, const vpColor &color, +void vpDisplay::displayPoint(const vpImage &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness) { vp_display_display_point(I, ip, color, thickness); @@ -440,8 +411,8 @@ void vpDisplay::displayPoint(const vpImage &I, \param color : Point color. \param thickness : Thickness of the point */ -void vpDisplay::displayPoint(const vpImage &I, int i, int j, - const vpColor &color, unsigned int thickness) +void vpDisplay::displayPoint(const vpImage &I, int i, int j, const vpColor &color, + unsigned int thickness) { vp_display_display_point(I, i, j, color, thickness); } @@ -453,8 +424,7 @@ void vpDisplay::displayPoint(const vpImage &I, int i, int j, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplay::displayPolygon(const vpImage &I, - const std::vector &vip, +void vpDisplay::displayPolygon(const vpImage &I, const std::vector &vip, const vpColor &color, unsigned int thickness) { vp_display_display_polygon(I, vip, color, thickness); @@ -474,14 +444,10 @@ void vpDisplay::displayPolygon(const vpImage &I, rectangle. This parameter is only useful when \e fill is set to false. */ -void vpDisplay::displayRectangle(const vpImage &I, - const vpImagePoint &topLeft, - unsigned int width, unsigned int height, - const vpColor &color, bool fill, - unsigned int thickness) +void vpDisplay::displayRectangle(const vpImage &I, const vpImagePoint &topLeft, unsigned int width, + unsigned int height, const vpColor &color, bool fill, unsigned int thickness) { - vp_display_display_rectangle(I, topLeft, width, height, color, fill, - thickness); + vp_display_display_rectangle(I, topLeft, width, height, color, fill, thickness); } /*! @@ -498,13 +464,10 @@ void vpDisplay::displayRectangle(const vpImage &I, rectangle. This parameter is only useful when \e fill is set to false. */ -void vpDisplay::displayRectangle(const vpImage &I, int i, - int j, unsigned int width, - unsigned int height, const vpColor &color, - bool fill, unsigned int thickness) +void vpDisplay::displayRectangle(const vpImage &I, int i, int j, unsigned int width, unsigned int height, + const vpColor &color, bool fill, unsigned int thickness) { - vp_display_display_rectangle(I, i, j, width, height, color, fill, - thickness); + vp_display_display_rectangle(I, i, j, width, height, color, fill, thickness); } /*! @@ -520,10 +483,8 @@ void vpDisplay::displayRectangle(const vpImage &I, int i, rectangle. This parameter is only useful when \e fill is set to false. */ -void vpDisplay::displayRectangle(const vpImage &I, - const vpRect &rectangle, - const vpColor &color, bool fill, - unsigned int thickness) +void vpDisplay::displayRectangle(const vpImage &I, const vpRect &rectangle, const vpColor &color, + bool fill, unsigned int thickness) { vp_display_display_rectangle(I, rectangle, color, fill, thickness); } @@ -541,13 +502,10 @@ void vpDisplay::displayRectangle(const vpImage &I, \param thickness : Thickness of the four lines used to display the rectangle. */ -void vpDisplay::displayRectangle(const vpImage &I, - const vpImagePoint ¢er, float angle, - unsigned int width, unsigned int height, - const vpColor &color, unsigned int thickness) +void vpDisplay::displayRectangle(const vpImage &I, const vpImagePoint ¢er, float angle, + unsigned int width, unsigned int height, const vpColor &color, unsigned int thickness) { - vp_display_display_rectangle(I, center, angle, width, height, color, - thickness); + vp_display_display_rectangle(I, center, angle, width, height, color, thickness); } /*! @@ -564,14 +522,11 @@ void vpDisplay::displayRectangle(const vpImage &I, rectangle. This parameter is only useful when \e fill is set to false. */ -void vpDisplay::displayRectangle(const vpImage &I, - const vpImagePoint &topLeft, - const vpImagePoint &bottomRight, - const vpColor &color, bool fill, +void vpDisplay::displayRectangle(const vpImage &I, const vpImagePoint &topLeft, + const vpImagePoint &bottomRight, const vpColor &color, bool fill, unsigned int thickness) { - vp_display_display_rectangle(I, topLeft, bottomRight, color, fill, - thickness); + vp_display_display_rectangle(I, topLeft, bottomRight, color, fill, thickness); } /*! @@ -587,13 +542,10 @@ void vpDisplay::displayRectangle(const vpImage &I, \param thickness : Thickness of the four lines used to display the rectangle. */ -void vpDisplay::displayRectangle(const vpImage &I, - unsigned int i, unsigned int j, float angle, - unsigned int width, unsigned int height, - const vpColor &color, unsigned int thickness) +void vpDisplay::displayRectangle(const vpImage &I, unsigned int i, unsigned int j, float angle, + unsigned int width, unsigned int height, const vpColor &color, unsigned int thickness) { - vp_display_display_rectangle(I, i, j, angle, width, height, color, - thickness); + vp_display_display_rectangle(I, i, j, angle, width, height, color, thickness); } /*! @@ -608,8 +560,7 @@ void vpDisplay::displayRectangle(const vpImage &I, \sa setFont() */ -void vpDisplay::displayText(const vpImage &I, - const vpImagePoint &ip, const std::string &s, +void vpDisplay::displayText(const vpImage &I, const vpImagePoint &ip, const std::string &s, const vpColor &color) { vp_display_display_text(I, ip, s, color); @@ -627,8 +578,7 @@ void vpDisplay::displayText(const vpImage &I, \sa setFont() */ -void vpDisplay::displayText(const vpImage &I, int i, int j, - const std::string &s, const vpColor &color) +void vpDisplay::displayText(const vpImage &I, int i, int j, const std::string &s, const vpColor &color) { vp_display_display_text(I, i, j, s, color); } @@ -662,10 +612,7 @@ int main() { \sa flushROI() */ -void vpDisplay::flush(const vpImage &I) -{ - vp_display_flush(I); -} +void vpDisplay::flush(const vpImage &I) { vp_display_flush(I); } /*! Flushes the output buffer associated to image \e I display. @@ -676,10 +623,7 @@ void vpDisplay::flush(const vpImage &I) \sa flush() */ -void vpDisplay::flushROI(const vpImage &I, const vpRect &roi) -{ - vp_display_flush_roi(I, roi); -} +void vpDisplay::flushROI(const vpImage &I, const vpRect &roi) { vp_display_flush_roi(I, roi); } /*! Display image \e I. @@ -692,19 +636,13 @@ void vpDisplay::flushROI(const vpImage &I, const vpRect &roi) \sa init(), close() */ -void vpDisplay::display(const vpImage &I) -{ - vp_display_display(I); -} +void vpDisplay::display(const vpImage &I) { vp_display_display(I); } /*! Update the display with the content of the image that is in the region of interest. \param I : Image. \param roi : Region of interest. */ -void vpDisplay::displayROI(const vpImage &I, const vpRect &roi) -{ - vp_display_display_roi(I, roi); -} +void vpDisplay::displayROI(const vpImage &I, const vpRect &roi) { vp_display_display_roi(I, roi); } /*! Wait for a click from one of the mouse button. @@ -723,10 +661,7 @@ void vpDisplay::displayROI(const vpImage &I, const vpRect &roi) - false if no button was clicked. This can occur if blocking is set to \e false. */ -bool vpDisplay::getClick(const vpImage &I, bool blocking) -{ - return vp_display_get_click(I, blocking); -} +bool vpDisplay::getClick(const vpImage &I, bool blocking) { return vp_display_get_click(I, blocking); } /*! Wait for a click from one of the mouse button and get the position @@ -746,8 +681,7 @@ bool vpDisplay::getClick(const vpImage &I, bool blocking) button is pressed, the location of the mouse pointer is updated in \e ip. */ -bool vpDisplay::getClick(const vpImage &I, vpImagePoint &ip, - bool blocking) +bool vpDisplay::getClick(const vpImage &I, vpImagePoint &ip, bool blocking) { return vp_display_get_click(I, ip, blocking); } @@ -772,8 +706,7 @@ bool vpDisplay::getClick(const vpImage &I, vpImagePoint &ip, button is pressed, the location of the mouse pointer is updated in \e ip. */ -bool vpDisplay::getClick(const vpImage &I, vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, +bool vpDisplay::getClick(const vpImage &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { return vp_display_get_click(I, ip, button, blocking); @@ -795,9 +728,7 @@ bool vpDisplay::getClick(const vpImage &I, vpImagePoint &ip, \return true if a mouse button is pressed, false otherwise. */ -bool vpDisplay::getClick(const vpImage &I, - vpMouseButton::vpMouseButtonType &button, - bool blocking) +bool vpDisplay::getClick(const vpImage &I, vpMouseButton::vpMouseButtonType &button, bool blocking) { vpImagePoint ip; return vpDisplay::getClick(I, ip, button, blocking); @@ -823,8 +754,7 @@ bool vpDisplay::getClick(const vpImage &I, button is released, the location of the mouse pointer is updated in \e ip. */ -bool vpDisplay::getClickUp(const vpImage &I, vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, +bool vpDisplay::getClickUp(const vpImage &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { return vp_display_get_click_up(I, ip, button, blocking); @@ -846,9 +776,7 @@ bool vpDisplay::getClickUp(const vpImage &I, vpImagePoint &ip, \return true if a mouse button is released, false otherwise. */ -bool vpDisplay::getClickUp(const vpImage &I, - vpMouseButton::vpMouseButtonType &button, - bool blocking) +bool vpDisplay::getClickUp(const vpImage &I, vpMouseButton::vpMouseButtonType &button, bool blocking) { vpImagePoint ip; return vpDisplay::getClickUp(I, ip, button, blocking); @@ -920,9 +848,12 @@ int main() // Non blocking keyboard event loop int cpt_event = 0; - std::cout << "Enter a non blocking keyboard event detection loop..." << -std::endl; do { bool event = vpDisplay::getKeyboardEvent(I, false); if (event) -{ std::cout << "A keyboard event was detected" << std::endl; cpt_event ++; + std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; + do { + bool event = vpDisplay::getKeyboardEvent(I, false); + if (event) { + std::cout << "A keyboard event was detected" << std::endl; + cpt_event ++; } vpTime::wait(5); // wait 5 ms @@ -934,8 +865,7 @@ std::endl; do { bool event = vpDisplay::getKeyboardEvent(I, false); if (event) } \endcode */ -bool vpDisplay::getKeyboardEvent(const vpImage &I, - bool blocking) +bool vpDisplay::getKeyboardEvent(const vpImage &I, bool blocking) { return vp_display_get_keyboard_event(I, blocking); } @@ -1009,9 +939,12 @@ int main() // Non blocking keyboard event loop int cpt_event = 0; std::string key; - std::cout << "Enter a non blocking keyboard event detection loop..." << -std::endl; do { bool event = vpDisplay::getKeyboardEvent(I, key, false); if -(event) { std::cout << "Key detected: " << key << std::endl; cpt_event ++; + std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; + do { + bool event = vpDisplay::getKeyboardEvent(I, key, false); + if (event) { + std::cout << "Key detected: " << key << std::endl; + cpt_event ++; } vpTime::wait(5); // wait 5 ms @@ -1023,8 +956,7 @@ std::endl; do { bool event = vpDisplay::getKeyboardEvent(I, key, false); if } \endcode */ -bool vpDisplay::getKeyboardEvent(const vpImage &I, - std::string &key, bool blocking) +bool vpDisplay::getKeyboardEvent(const vpImage &I, std::string &key, bool blocking) { return vp_display_get_keyboard_event(I, key, blocking); } @@ -1098,8 +1030,9 @@ int main() // Non blocking keyboard event loop int cpt_event = 0; char key[10]; - std::cout << "Enter a non blocking keyboard event detection loop..." << -std::endl; do { bool event = vpDisplay::getKeyboardEvent(I, &key[0], false); + std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; + do { + bool event = vpDisplay::getKeyboardEvent(I, &key[0], false); if (event) { std::cout << "Key detected: " << key << std::endl; cpt_event ++; @@ -1114,8 +1047,7 @@ std::endl; do { bool event = vpDisplay::getKeyboardEvent(I, &key[0], false); } \endcode */ -bool vpDisplay::getKeyboardEvent(const vpImage &I, char *key, - bool blocking) +bool vpDisplay::getKeyboardEvent(const vpImage &I, char *key, bool blocking) { return vp_display_get_keyboard_event(I, key, blocking); } @@ -1129,8 +1061,7 @@ bool vpDisplay::getKeyboardEvent(const vpImage &I, char *key, \return true if a pointer motion event was received, false otherwise. */ -bool vpDisplay::getPointerMotionEvent(const vpImage &I, - vpImagePoint &ip) +bool vpDisplay::getPointerMotionEvent(const vpImage &I, vpImagePoint &ip) { return vp_display_get_pointer_motion_event(I, ip); } @@ -1144,8 +1075,7 @@ bool vpDisplay::getPointerMotionEvent(const vpImage &I, \return true. */ -bool vpDisplay::getPointerPosition(const vpImage &I, - vpImagePoint &ip) +bool vpDisplay::getPointerPosition(const vpImage &I, vpImagePoint &ip) { return vp_display_get_pointer_position(I, ip); } @@ -1159,8 +1089,7 @@ bool vpDisplay::getPointerPosition(const vpImage &I, \exception vpDisplayException::notInitializedError : If the video device is not initialized. */ -void vpDisplay::setBackground(const vpImage &I, - const vpColor &color) +void vpDisplay::setBackground(const vpImage &I, const vpColor &color) { vp_display_set_background(I, color); } @@ -1178,8 +1107,7 @@ void vpDisplay::setBackground(const vpImage &I, \sa displayText() */ -void vpDisplay::setFont(const vpImage &I, - const std::string &fontname) +void vpDisplay::setFont(const vpImage &I, const std::string &fontname) { vp_display_set_font(I, fontname); } @@ -1191,8 +1119,7 @@ void vpDisplay::setFont(const vpImage &I, \param I : Image associated to the display window. \param windowtitle : Window title. */ -void vpDisplay::setTitle(const vpImage &I, - const std::string &windowtitle) +void vpDisplay::setTitle(const vpImage &I, const std::string &windowtitle) { vp_display_set_title(I, windowtitle); } @@ -1207,8 +1134,7 @@ void vpDisplay::setTitle(const vpImage &I, \exception vpDisplayException::notInitializedError : If the video device is not initialized. */ -void vpDisplay::setWindowPosition(const vpImage &I, int winx, - int winy) +void vpDisplay::setWindowPosition(const vpImage &I, int winx, int winy) { vp_display_set_window_position(I, winx, winy); } diff --git a/modules/core/src/display/vpFeatureDisplay.cpp b/modules/core/src/display/vpFeatureDisplay.cpp index 44869a3d76..d285416185 100644 --- a/modules/core/src/display/vpFeatureDisplay.cpp +++ b/modules/core/src/display/vpFeatureDisplay.cpp @@ -61,11 +61,8 @@ \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 &I, - const vpColor &color, - unsigned int thickness) +void vpFeatureDisplay::displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage &I, + const vpColor &color, unsigned int thickness) { try { vpImagePoint ip; // pixel coordinates in float @@ -84,11 +81,8 @@ void vpFeatureDisplay::displayPoint(double x, double y, \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 &I, - const vpColor &color, - unsigned int thickness) +void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParameters &cam, + const vpImage &I, const vpColor &color, unsigned int thickness) { try { @@ -134,12 +128,9 @@ void vpFeatureDisplay::displayLine(double rho, double theta, \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 &I, - const vpColor &color, - unsigned int thickness) +void vpFeatureDisplay::displayCylinder(double rho1, double theta1, double rho2, double theta2, + const vpCameraParameters &cam, const vpImage &I, + const vpColor &color, unsigned int thickness) { try { displayLine(rho1, theta1, cam, I, color, thickness); @@ -161,17 +152,13 @@ void vpFeatureDisplay::displayCylinder(double rho1, double theta1, \sa vpDisplay::displayEllipse() */ -void vpFeatureDisplay::displayEllipse(double x, double y, double mu20, - double mu11, double mu02, - const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color, - unsigned int thickness) +void vpFeatureDisplay::displayEllipse(double x, double y, double mu20, double mu11, double mu02, + const vpCameraParameters &cam, const vpImage &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 + 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); @@ -232,11 +219,8 @@ void vpFeatureDisplay::displayEllipse(double x, double y, double mu20, \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 &I, - const vpColor &color, - unsigned int thickness) +void vpFeatureDisplay::displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage &I, + const vpColor &color, unsigned int thickness) { try { vpImagePoint ip; // pixel coordinates in float @@ -256,11 +240,8 @@ void vpFeatureDisplay::displayPoint(double x, double y, \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 &I, - const vpColor &color, - unsigned int thickness) +void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage &I, + const vpColor &color, unsigned int thickness) { try { @@ -306,11 +287,8 @@ void vpFeatureDisplay::displayLine(double rho, double theta, \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 &I, - const vpColor &color, +void vpFeatureDisplay::displayCylinder(double rho1, double theta1, double rho2, double theta2, + const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) { try { @@ -334,17 +312,13 @@ void vpFeatureDisplay::displayCylinder(double rho1, double theta1, \sa vpDisplay::displayEllipse() */ -void vpFeatureDisplay::displayEllipse(double x, double y, double mu20, - double mu11, double mu02, - const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color, +void vpFeatureDisplay::displayEllipse(double x, double y, double mu20, double mu11, double mu02, + const vpCameraParameters &cam, const vpImage &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 + 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); diff --git a/modules/core/src/image/vpImageConvert.cpp b/modules/core/src/image/vpImageConvert.cpp index 29e2eb0269..41f6adc1fd 100644 --- a/modules/core/src/image/vpImageConvert.cpp +++ b/modules/core/src/image/vpImageConvert.cpp @@ -50,8 +50,7 @@ #include #include -#if defined __SSE2__ || defined _M_X64 || \ - (defined _M_IX86_FP && _M_IX86_FP >= 2) +#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2) #include #define VISP_HAVE_SSE2 1 @@ -77,13 +76,11 @@ int vpImageConvert::vpCbb[256]; \param src : source image \param dest : destination image */ -void vpImageConvert::convert(const vpImage &src, - vpImage &dest) +void vpImageConvert::convert(const vpImage &src, vpImage &dest) { dest.resize(src.getHeight(), src.getWidth()); - GreyToRGBa(src.bitmap, (unsigned char *)dest.bitmap, - src.getHeight() * src.getWidth()); + GreyToRGBa(src.bitmap, (unsigned char *)dest.bitmap, src.getHeight() * src.getWidth()); } /*! @@ -91,21 +88,18 @@ void vpImageConvert::convert(const vpImage &src, \param src : source image \param dest : destination image */ -void vpImageConvert::convert(const vpImage &src, - vpImage &dest) +void vpImageConvert::convert(const vpImage &src, vpImage &dest) { dest.resize(src.getHeight(), src.getWidth()); - RGBaToGrey((unsigned char *)src.bitmap, dest.bitmap, - src.getHeight() * src.getWidth()); + RGBaToGrey((unsigned char *)src.bitmap, dest.bitmap, src.getHeight() * src.getWidth()); } /*! Convert a vpImage\ to a vpImage\ by renormalizing between 0 and 255. \param src : source image \param dest : destination image */ -void vpImageConvert::convert(const vpImage &src, - vpImage &dest) +void vpImageConvert::convert(const vpImage &src, vpImage &dest) { dest.resize(src.getHeight(), src.getWidth()); unsigned int max_xy = src.getWidth() * src.getHeight(); @@ -129,8 +123,7 @@ Convert a vpImage\ to a vpImage\ by basic casting. \param src : source image \param dest : destination image */ -void vpImageConvert::convert(const vpImage &src, - vpImage &dest) +void vpImageConvert::convert(const vpImage &src, vpImage &dest) { dest.resize(src.getHeight(), src.getWidth()); for (unsigned int i = 0; i < src.getHeight() * src.getWidth(); i++) @@ -141,8 +134,7 @@ void vpImageConvert::convert(const vpImage &src, Convert a vpImage\ to a vpImage\ by renormalizing between 0 and 255. \param src : source image \param dest : destination image */ -void vpImageConvert::convert(const vpImage &src, - vpImage &dest) +void vpImageConvert::convert(const vpImage &src, vpImage &dest) { dest.resize(src.getHeight(), src.getWidth()); unsigned int max_xy = src.getWidth() * src.getHeight(); @@ -166,8 +158,7 @@ Convert a vpImage\ to a vpImage\. \param src : source image \param dest : destination image */ -void vpImageConvert::convert(const vpImage &src, - vpImage &dest) +void vpImageConvert::convert(const vpImage &src, vpImage &dest) { dest.resize(src.getHeight(), src.getWidth()); @@ -180,8 +171,7 @@ Convert a vpImage\ to a vpImage\. \param src : source image \param dest : destination image */ -void vpImageConvert::convert(const vpImage &src, - vpImage &dest) +void vpImageConvert::convert(const vpImage &src, vpImage &dest) { dest.resize(src.getHeight(), src.getWidth()); @@ -194,8 +184,7 @@ Convert a vpImage\ to a vpImage\ by basic casting. \param src : source image \param dest : destination image */ -void vpImageConvert::convert(const vpImage &src, - vpImage &dest) +void vpImageConvert::convert(const vpImage &src, vpImage &dest) { dest.resize(src.getHeight(), src.getWidth()); for (unsigned int i = 0; i < src.getHeight() * src.getWidth(); i++) @@ -209,8 +198,7 @@ void vpImageConvert::convert(const vpImage &src, \param src_depth : input 16-bits depth image. \param dest_rgba : output color depth image. */ -void vpImageConvert::createDepthHistogram(const vpImage &src_depth, - vpImage &dest_rgba) +void vpImageConvert::createDepthHistogram(const vpImage &src_depth, vpImage &dest_rgba) { dest_rgba.resize(src_depth.getHeight(), src_depth.getWidth()); static uint32_t histogram[0x10000]; @@ -225,8 +213,7 @@ void vpImageConvert::createDepthHistogram(const vpImage &src_depth, for (unsigned int i = 0; i < src_depth.getSize(); ++i) { uint16_t d = src_depth.bitmap[i]; if (d) { - int f = (int)(histogram[d] * 255 / - histogram[0xFFFF]); // 0-255 based on histogram location + int f = (int)(histogram[d] * 255 / histogram[0xFFFF]); // 0-255 based on histogram location dest_rgba.bitmap[i].R = 255 - f; dest_rgba.bitmap[i].G = 0; dest_rgba.bitmap[i].B = f; @@ -246,8 +233,7 @@ void vpImageConvert::createDepthHistogram(const vpImage &src_depth, src_depth : input 16-bits depth image. \param dest_depth : output grayscale depth image. */ -void vpImageConvert::createDepthHistogram(const vpImage &src_depth, - vpImage &dest_depth) +void vpImageConvert::createDepthHistogram(const vpImage &src_depth, vpImage &dest_depth) { dest_depth.resize(src_depth.getHeight(), src_depth.getWidth()); static uint32_t histogram2[0x10000]; @@ -262,8 +248,7 @@ void vpImageConvert::createDepthHistogram(const vpImage &src_depth, for (unsigned int i = 0; i < src_depth.getSize(); ++i) { uint16_t d = src_depth.bitmap[i]; if (d) { - int f = (int)(histogram2[d] * 255 / - histogram2[0xFFFF]); // 0-255 based on histogram location + int f = (int)(histogram2[d] * 255 / histogram2[0xFFFF]); // 0-255 based on histogram location dest_depth.bitmap[i] = f; } else { dest_depth.bitmap[i] = 0; @@ -319,8 +304,7 @@ int main() } \endcode */ -void vpImageConvert::convert(const IplImage *src, vpImage &dest, - bool flip) +void vpImageConvert::convert(const IplImage *src, vpImage &dest, bool flip) { int nChannel = src->nChannels; int depth = src->depth; @@ -338,9 +322,7 @@ void vpImageConvert::convert(const IplImage *src, vpImage &dest, for (int i = 0; i < height; i++) { unsigned char *line = input; - unsigned char *output = beginOutput + - lineStep * (4 * width * (height - 1 - i)) + - (1 - lineStep) * 4 * width * i; + unsigned char *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); @@ -360,9 +342,7 @@ void vpImageConvert::convert(const IplImage *src, vpImage &dest, for (int i = 0; i < height; i++) { unsigned char *line = input; - unsigned char *output = beginOutput + - lineStep * (4 * width * (height - 1 - i)) + - (1 - lineStep) * 4 * width * i; + unsigned char *output = beginOutput + lineStep * (4 * width * (height - 1 - i)) + (1 - lineStep) * 4 * width * i; for (int j = 0; j < width; j++) { *output++ = *(line); *output++ = *(line); @@ -420,8 +400,7 @@ int main() } \endcode */ -void vpImageConvert::convert(const IplImage *src, - vpImage &dest, bool flip) +void vpImageConvert::convert(const IplImage *src, vpImage &dest, bool flip) { int nChannel = src->nChannels; int depth = src->depth; @@ -438,22 +417,20 @@ void vpImageConvert::convert(const IplImage *src, } 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); + 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); + 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); + BGRToGrey((unsigned char *)src->imageData + i * widthStep, dest.bitmap + i * width, (unsigned int)width, 1, + false); } } } @@ -462,15 +439,13 @@ void vpImageConvert::convert(const IplImage *src, 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); + 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, + BGRToGrey((unsigned char *)src->imageData /*+ i*widthStep*/, dest.bitmap /*+ i*width*/, (unsigned int)width, (unsigned int)height /*1*/, true); //} } @@ -529,8 +504,7 @@ void vpImageConvert::convert(const vpImage &src, IplImage *&dest) 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 != channels || dest->depth != depth || dest->height != height || dest->width != width) { if (dest->nChannels != 0) cvReleaseImage(&dest); dest = cvCreateImage(size, depth, channels); @@ -605,8 +579,7 @@ int main() } \endcode */ -void vpImageConvert::convert(const vpImage &src, - IplImage *&dest) +void vpImageConvert::convert(const vpImage &src, IplImage *&dest) { unsigned int height = src.getHeight(); unsigned int width = src.getWidth(); @@ -614,8 +587,8 @@ void vpImageConvert::convert(const vpImage &src, 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 != channels || dest->depth != depth || dest->height != (int)height || + dest->width != (int)width) { if (dest->nChannels != 0) cvReleaseImage(&dest); dest = cvCreateImage(size, depth, channels); @@ -681,8 +654,7 @@ int main() } \endcode */ -void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, - const bool flip) +void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, const bool flip) { if (src.type() == CV_8UC4) { dest.resize((unsigned int)src.rows, (unsigned int)src.cols); @@ -774,8 +746,7 @@ int main() \endcode */ -void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, - const bool flip) +void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, const bool flip) { if (src.type() == CV_8UC1) { dest.resize((unsigned int)src.rows, (unsigned int)src.cols); @@ -784,34 +755,29 @@ void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, } 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); + 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); + 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); + 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 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(), + BGRToGrey((unsigned char *)src.data + i * src.step1(), (unsigned char *)dest.bitmap + i * dest.getCols(), (unsigned int)dest.getCols(), 1, false); } } @@ -860,8 +826,7 @@ int main() */ void vpImageConvert::convert(const vpImage &src, cv::Mat &dest) { - cv::Mat vpToMat((int)src.getRows(), (int)src.getCols(), CV_8UC4, - (void *)src.bitmap); + 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); @@ -912,16 +877,13 @@ int main() \endcode */ -void vpImageConvert::convert(const vpImage &src, cv::Mat &dest, - const bool copyData) +void vpImageConvert::convert(const vpImage &src, cv::Mat &dest, const bool copyData) { if (copyData) { - cv::Mat tmpMap((int)src.getRows(), (int)src.getCols(), CV_8UC1, - (void *)src.bitmap); + 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); + dest = cv::Mat((int)src.getRows(), (int)src.getCols(), CV_8UC1, (void *)src.bitmap); } } @@ -955,18 +917,16 @@ int main() // 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\ to a -yarp::sig::ImageOf\ vpImageConvert::convert(I, Iyarp); + yarp::sig::ImageOf< yarp::sig::PixelMono > *Iyarp = new yarp::sig::ImageOf(); + // Convert the vpImage\ to a yarp::sig::ImageOf\ + vpImageConvert::convert(I, Iyarp); // ... #endif } \endcode */ -void vpImageConvert::convert(const vpImage &src, - yarp::sig::ImageOf *dest, +void vpImageConvert::convert(const vpImage &src, yarp::sig::ImageOf *dest, const bool copyData) { if (copyData) { @@ -1002,13 +962,12 @@ only update the image pointer. int main() { #if defined(VISP_HAVE_YARP) - yarp::sig::ImageOf< yarp::sig::PixelMono > *Iyarp = new yarp::sig::ImageOf< -yarp::sig::PixelMono >(); + yarp::sig::ImageOf< yarp::sig::PixelMono > *Iyarp = new yarp::sig::ImageOf(); // Read an image on a disk yarp::sig::file::read(*Iyarp, "image.pgm"); - // Convert the yarp::sig::ImageOf to a -vpImage vpImage I; + // Convert the yarp::sig::ImageOf to a vpImage + vpImage I; vpImageConvert::convert(Iyarp, I); // ... @@ -1016,14 +975,12 @@ vpImage vpImage I; } \endcode */ -void vpImageConvert::convert( - const yarp::sig::ImageOf *src, - vpImage &dest, const bool copyData) +void vpImageConvert::convert(const yarp::sig::ImageOf *src, vpImage &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)); + memcpy(dest.bitmap, src->getRawImage(), src->height() * src->width() * sizeof(yarp::sig::PixelMono)); else dest.bitmap = src->getRawImage(); } @@ -1054,8 +1011,7 @@ int main() // 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 >(); + yarp::sig::ImageOf< yarp::sig::PixelRgba > *Iyarp = new yarp::sig::ImageOf(); // Convert the vpImage to a yarp::sig::ImageOf vpImageConvert::convert(I,Iyarp); @@ -1064,14 +1020,12 @@ yarp::sig::PixelRgba >(); } \endcode */ -void vpImageConvert::convert(const vpImage &src, - yarp::sig::ImageOf *dest, +void vpImageConvert::convert(const vpImage &src, yarp::sig::ImageOf *dest, const bool copyData) { if (copyData) { dest->resize(src.getWidth(), src.getHeight()); - memcpy(dest->getRawImage(), src.bitmap, - src.getHeight() * src.getWidth() * sizeof(vpRGBa)); + memcpy(dest->getRawImage(), src.bitmap, src.getHeight() * src.getWidth() * sizeof(vpRGBa)); } else dest->setExternal(src.bitmap, (int)src.getCols(), (int)src.getRows()); } @@ -1102,8 +1056,7 @@ only update the image pointer. int main() { #if defined(VISP_HAVE_YARP) - yarp::sig::ImageOf< yarp::sig::PixelRgba > *Iyarp = new yarp::sig::ImageOf< -yarp::sig::PixelRgba >(); + yarp::sig::ImageOf< yarp::sig::PixelRgba > *Iyarp = new yarp::sig::ImageOf(); // Read an image on a disk yarp::sig::file::read(*Iyarp,"image.pgm"); @@ -1116,14 +1069,12 @@ yarp::sig::PixelRgba >(); } \endcode */ -void vpImageConvert::convert( - const yarp::sig::ImageOf *src, - vpImage &dest, const bool copyData) +void vpImageConvert::convert(const yarp::sig::ImageOf *src, vpImage &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)); + memcpy(dest.bitmap, src->getRawImage(), src->height() * src->width() * sizeof(yarp::sig::PixelRgba)); else dest.bitmap = static_cast(src->getRawImage()); } @@ -1152,8 +1103,7 @@ int main() // 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 >(); + yarp::sig::ImageOf< yarp::sig::PixelRgb > *Iyarp = new yarp::sig::ImageOf(); // Convert the vpImage to a yarp::sig::ImageOf vpImageConvert::convert(I,Iyarp); @@ -1162,8 +1112,7 @@ yarp::sig::PixelRgb >(); } \endcode */ -void vpImageConvert::convert(const vpImage &src, - yarp::sig::ImageOf *dest) +void vpImageConvert::convert(const vpImage &src, yarp::sig::ImageOf *dest) { dest->resize(src.getWidth(), src.getHeight()); for (unsigned int i = 0; i < src.getRows(); i++) { @@ -1201,8 +1150,7 @@ YARP image class documentation. int main() { #if defined(VISP_HAVE_YARP) - yarp::sig::ImageOf< yarp::sig::PixelRgb > *Iyarp = new yarp::sig::ImageOf< -yarp::sig::PixelRgb >(); + yarp::sig::ImageOf< yarp::sig::PixelRgb > *Iyarp = new yarp::sig::ImageOf(); // Read an image on a disk yarp::sig::file::read(*Iyarp,"image.pgm"); @@ -1215,8 +1163,7 @@ yarp::sig::PixelRgb >(); } \endcode */ -void vpImageConvert::convert( - const yarp::sig::ImageOf *src, vpImage &dest) +void vpImageConvert::convert(const yarp::sig::ImageOf *src, vpImage &dest) { dest.resize(src->height(), src->width()); for (int i = 0; i < src->height(); i++) { @@ -1231,12 +1178,12 @@ void vpImageConvert::convert( #endif -#define vpSAT(c) \ - if (c & (~255)) { \ - if (c < 0) \ - c = 0; \ - else \ - c = 255; \ +#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. @@ -1246,8 +1193,7 @@ void vpImageConvert::convert( \sa YUV422ToRGBa() */ -void vpImageConvert::YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, - unsigned int width, unsigned int height) +void vpImageConvert::YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, unsigned int width, unsigned int height) { unsigned char *s; unsigned char *d; @@ -1302,8 +1248,7 @@ void vpImageConvert::YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, \sa YUV422ToRGB() */ -void vpImageConvert::YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, - unsigned int width, unsigned int height) +void vpImageConvert::YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, unsigned int width, unsigned int height) { unsigned char *s; unsigned char *d; @@ -1355,8 +1300,7 @@ void vpImageConvert::YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, \sa YUV422ToGrey() */ -void vpImageConvert::YUYVToGrey(unsigned char *yuyv, unsigned char *grey, - unsigned int size) +void vpImageConvert::YUYVToGrey(unsigned char *yuyv, unsigned char *grey, unsigned int size) { unsigned int i = 0, j = 0; @@ -1373,8 +1317,7 @@ void vpImageConvert::YUYVToGrey(unsigned char *yuyv, unsigned char *grey, component of the converted image is set to vpRGBa::alpha_default. */ -void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, - unsigned int size) +void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { #if 1 // std::cout << "call optimized ConvertYUV411ToRGBa()" << std::endl; @@ -1535,8 +1478,7 @@ void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, \sa YUYVToRGBa() */ -void vpImageConvert::YUV422ToRGBa(unsigned char *yuv, unsigned char *rgba, - unsigned int size) +void vpImageConvert::YUV422ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { #if 1 @@ -1630,8 +1572,7 @@ Convert YUV411 into Grey yuv411 : u y1 y2 v y3 y4 */ -void vpImageConvert::YUV411ToGrey(unsigned char *yuv, unsigned char *grey, - unsigned int size) +void vpImageConvert::YUV411ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) { unsigned int i = 0, j = 0; while (j < size * 3 / 2) { @@ -1654,8 +1595,7 @@ void vpImageConvert::YUV411ToGrey(unsigned char *yuv, unsigned char *grey, \sa YUYVToRGB() */ -void vpImageConvert::YUV422ToRGB(unsigned char *yuv, unsigned char *rgb, - unsigned int size) +void vpImageConvert::YUV422ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { #if 1 // std::cout << "call optimized convertYUV422ToRGB()" << std::endl; @@ -1746,8 +1686,7 @@ void vpImageConvert::YUV422ToRGB(unsigned char *yuv, unsigned char *rgb, \sa YUYVToGrey() */ -void vpImageConvert::YUV422ToGrey(unsigned char *yuv, unsigned char *grey, - unsigned int size) +void vpImageConvert::YUV422ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) { unsigned int i = 0, j = 0; @@ -1764,8 +1703,7 @@ Convert YUV411 into RGB yuv411 : u y1 y2 v y3 y4 */ -void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, - unsigned int size) +void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { #if 1 // std::cout << "call optimized ConvertYUV411ToRGB()" << std::endl; @@ -1919,8 +1857,7 @@ void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, The alpha component of the converted image is set to vpRGBa::alpha_default. */ -void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, - unsigned int width, unsigned int height) +void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) { // std::cout << "call optimized ConvertYUV420ToRGBa()" << std::endl; int U, V, R, G, B, V2, U5, UV; @@ -2052,8 +1989,7 @@ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, Convert YUV420 [Y(NxM), U(N/2xM/2), V(N/2xM/2)] image into RGB image. */ -void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, - unsigned int width, unsigned int height) +void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height) { // std::cout << "call optimized ConvertYUV420ToRGB()" << std::endl; int U, V, R, G, B, V2, U5, UV; @@ -2182,8 +2118,7 @@ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, Convert YUV420 [Y(NxM), U(N/2xM/2), V(N/2xM/2)] image into grey image. */ -void vpImageConvert::YUV420ToGrey(unsigned char *yuv, unsigned char *grey, - unsigned int size) +void vpImageConvert::YUV420ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) { for (unsigned int i = 0; i < size; i++) { *grey++ = *yuv++; @@ -2196,8 +2131,7 @@ void vpImageConvert::YUV420ToGrey(unsigned char *yuv, unsigned char *grey, The alpha component of the converted image is set to vpRGBa::alpha_default. */ -void vpImageConvert::YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, - unsigned int size) +void vpImageConvert::YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { for (unsigned int i = 0; i < size; i++) { int U = (int)((*yuv++ - 128) * 0.354); @@ -2240,8 +2174,7 @@ void vpImageConvert::YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, Convert YUV444 (u y v) image into RGB image. */ -void vpImageConvert::YUV444ToRGB(unsigned char *yuv, unsigned char *rgb, - unsigned int size) +void vpImageConvert::YUV444ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { for (unsigned int i = 0; i < size; i++) { int U = (int)((*yuv++ - 128) * 0.354); @@ -2284,8 +2217,7 @@ void vpImageConvert::YUV444ToRGB(unsigned char *yuv, unsigned char *rgb, Convert YUV444 (u y v) image into grey image. */ -void vpImageConvert::YUV444ToGrey(unsigned char *yuv, unsigned char *grey, - unsigned int size) +void vpImageConvert::YUV444ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) { yuv++; for (unsigned int i = 0; i < size; i++) { @@ -2301,8 +2233,7 @@ void vpImageConvert::YUV444ToGrey(unsigned char *yuv, unsigned char *grey, The alpha component of the converted image is set to vpRGBa::alpha_default. */ -void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, - unsigned int width, unsigned int height) +void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) { // std::cout << "call optimized ConvertYV12ToRGBa()" << std::endl; int U, V, R, G, B, V2, U5, UV; @@ -2434,8 +2365,7 @@ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, Convert YV12 [Y(NxM), V(N/2xM/2), U(N/2xM/2)] image into RGB image. */ -void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, - unsigned int height, unsigned int width) +void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int height, unsigned int width) { // std::cout << "call optimized ConvertYV12ToRGB()" << std::endl; int U, V, R, G, B, V2, U5, UV; @@ -2566,8 +2496,7 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, The alpha component of the converted image is set to vpRGBa::alpha_default. */ -void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, - unsigned int width, unsigned int height) +void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) { // std::cout << "call optimized ConvertYVU9ToRGBa()" << std::endl; int U, V, R, G, B, V2, U5, UV; @@ -3000,8 +2929,7 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, Convert YV12 [Y(NxM), V(N/4xM/4), U(N/4xM/4)] image into RGB image. */ -void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, - unsigned int height, unsigned int width) +void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int height, unsigned int width) { // std::cout << "call optimized ConvertYVU9ToRGB()" << std::endl; int U, V, R, G, B, V2, U5, UV; @@ -3421,8 +3349,7 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, Alpha component is set to vpRGBa::alpha_default. */ -void vpImageConvert::RGBToRGBa(unsigned char *rgb, unsigned char *rgba, - unsigned int size) +void vpImageConvert::RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned int size) { unsigned char *pt_input = rgb; unsigned char *pt_end = rgb + 3 * size; @@ -3443,8 +3370,7 @@ void vpImageConvert::RGBToRGBa(unsigned char *rgb, unsigned char *rgba, The alpha component of the converted image is set to vpRGBa::alpha_default. */ -void vpImageConvert::RGBaToRGB(unsigned char *rgba, unsigned char *rgb, - unsigned int size) +void vpImageConvert::RGBaToRGB(unsigned char *rgba, unsigned char *rgb, unsigned int size) { unsigned char *pt_input = rgba; unsigned char *pt_end = rgba + 4 * size; @@ -3463,8 +3389,7 @@ void vpImageConvert::RGBaToRGB(unsigned char *rgba, unsigned char *rgb, http://www.poynton.com/notes/colour_and_gamma/ColorFAQ.html */ -void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, - unsigned int size) +void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int size) { bool checkSSSE3 = vpCPUFeatures::checkSSSE3(); #if !VISP_HAVE_SSSE3 @@ -3477,50 +3402,32 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, if (size >= 16) { // Mask to select R component - const __m128i mask_R1 = _mm_set_epi8(-1, -1, -1, -1, 15, -1, 12, -1, 9, - -1, 6, -1, 3, -1, 0, -1); - const __m128i mask_R2 = _mm_set_epi8(5, -1, 2, -1, -1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1, -1); - const __m128i mask_R3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, - -1, 14, -1, 11, -1, 8, -1); - const __m128i mask_R4 = _mm_set_epi8(13, -1, 10, -1, 7, -1, 4, -1, 1, - -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_R1 = _mm_set_epi8(-1, -1, -1, -1, 15, -1, 12, -1, 9, -1, 6, -1, 3, -1, 0, -1); + const __m128i mask_R2 = _mm_set_epi8(5, -1, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_R3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 14, -1, 11, -1, 8, -1); + const __m128i mask_R4 = _mm_set_epi8(13, -1, 10, -1, 7, -1, 4, -1, 1, -1, -1, -1, -1, -1, -1, -1); // Mask to select G component - const __m128i mask_G1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, 13, -1, 10, - -1, 7, -1, 4, -1, 1, -1); - const __m128i mask_G2 = _mm_set_epi8(6, -1, 3, -1, 0, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1, -1); - const __m128i mask_G3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, - -1, 15, -1, 12, -1, 9, -1); - const __m128i mask_G4 = _mm_set_epi8(14, -1, 11, -1, 8, -1, 5, -1, 2, - -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_G1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, 13, -1, 10, -1, 7, -1, 4, -1, 1, -1); + const __m128i mask_G2 = _mm_set_epi8(6, -1, 3, -1, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_G3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 15, -1, 12, -1, 9, -1); + const __m128i mask_G4 = _mm_set_epi8(14, -1, 11, -1, 8, -1, 5, -1, 2, -1, -1, -1, -1, -1, -1, -1); // Mask to select B component - const __m128i mask_B1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, 14, -1, 11, - -1, 8, -1, 5, -1, 2, -1); - const __m128i mask_B2 = _mm_set_epi8(7, -1, 4, -1, 1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1, -1); - const __m128i mask_B3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, - -1, -1, -1, 13, -1, 10, -1); - const __m128i mask_B4 = _mm_set_epi8(15, -1, 12, -1, 9, -1, 6, -1, 3, - -1, 0, -1, -1, -1, -1, -1); + const __m128i mask_B1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, 14, -1, 11, -1, 8, -1, 5, -1, 2, -1); + const __m128i mask_B2 = _mm_set_epi8(7, -1, 4, -1, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_B3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 13, -1, 10, -1); + const __m128i mask_B4 = _mm_set_epi8(15, -1, 12, -1, 9, -1, 6, -1, 3, -1, 0, -1, -1, -1, -1, -1); // Mask to select the gray component - const __m128i mask_low1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, - 15, 13, 11, 9, 7, 5, 3, 1); - const __m128i mask_low2 = _mm_set_epi8(15, 13, 11, 9, 7, 5, 3, 1, -1, - -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_low1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, 15, 13, 11, 9, 7, 5, 3, 1); + const __m128i mask_low2 = _mm_set_epi8(15, 13, 11, 9, 7, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1, -1); // Coefficients RGB to Gray - const __m128i coeff_R = _mm_set_epi16(13933, 13933, 13933, 13933, 13933, - 13933, 13933, 13933); - const __m128i coeff_G = - _mm_set_epi16((short int)46871, (short int)46871, (short int)46871, - (short int)46871, (short int)46871, (short int)46871, - (short int)46871, (short int)46871); - const __m128i coeff_B = - _mm_set_epi16(4732, 4732, 4732, 4732, 4732, 4732, 4732, 4732); + const __m128i coeff_R = _mm_set_epi16(13933, 13933, 13933, 13933, 13933, 13933, 13933, 13933); + const __m128i coeff_G = _mm_set_epi16((short int)46871, (short int)46871, (short int)46871, (short int)46871, + (short int)46871, (short int)46871, (short int)46871, (short int)46871); + const __m128i coeff_B = _mm_set_epi16(4732, 4732, 4732, 4732, 4732, 4732, 4732, 4732); for (; i <= size - 16; i += 16) { // Process 16 color pixels @@ -3528,40 +3435,24 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, const __m128i data2 = _mm_loadu_si128((const __m128i *)(rgb + 16)); const __m128i data3 = _mm_loadu_si128((const __m128i *)(rgb + 32)); - const __m128i red_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_R1), - _mm_shuffle_epi8(data2, mask_R2)); - const __m128i green_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_G1), - _mm_shuffle_epi8(data2, mask_G2)); - const __m128i blue_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_B1), - _mm_shuffle_epi8(data2, mask_B2)); - - const __m128i grays_0_7 = _mm_adds_epu16( - _mm_mulhi_epu16(red_0_7, coeff_R), - _mm_adds_epu16(_mm_mulhi_epu16(green_0_7, coeff_G), - _mm_mulhi_epu16(blue_0_7, coeff_B))); - - const __m128i red_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data2, mask_R3), - _mm_shuffle_epi8(data3, mask_R4)); - const __m128i green_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data2, mask_G3), - _mm_shuffle_epi8(data3, mask_G4)); - const __m128i blue_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data2, mask_B3), - _mm_shuffle_epi8(data3, mask_B4)); - - const __m128i grays_8_15 = _mm_adds_epu16( - _mm_mulhi_epu16(red_8_15, coeff_R), - _mm_adds_epu16(_mm_mulhi_epu16(green_8_15, coeff_G), - _mm_mulhi_epu16(blue_8_15, coeff_B))); - - _mm_storeu_si128( - (__m128i *)grey, - _mm_or_si128(_mm_shuffle_epi8(grays_0_7, mask_low1), - _mm_shuffle_epi8(grays_8_15, mask_low2))); + const __m128i red_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_R1), _mm_shuffle_epi8(data2, mask_R2)); + const __m128i green_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_G1), _mm_shuffle_epi8(data2, mask_G2)); + const __m128i blue_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_B1), _mm_shuffle_epi8(data2, mask_B2)); + + const __m128i grays_0_7 = + _mm_adds_epu16(_mm_mulhi_epu16(red_0_7, coeff_R), + _mm_adds_epu16(_mm_mulhi_epu16(green_0_7, coeff_G), _mm_mulhi_epu16(blue_0_7, coeff_B))); + + const __m128i red_8_15 = _mm_or_si128(_mm_shuffle_epi8(data2, mask_R3), _mm_shuffle_epi8(data3, mask_R4)); + const __m128i green_8_15 = _mm_or_si128(_mm_shuffle_epi8(data2, mask_G3), _mm_shuffle_epi8(data3, mask_G4)); + const __m128i blue_8_15 = _mm_or_si128(_mm_shuffle_epi8(data2, mask_B3), _mm_shuffle_epi8(data3, mask_B4)); + + const __m128i grays_8_15 = + _mm_adds_epu16(_mm_mulhi_epu16(red_8_15, coeff_R), + _mm_adds_epu16(_mm_mulhi_epu16(green_8_15, coeff_G), _mm_mulhi_epu16(blue_8_15, coeff_B))); + + _mm_storeu_si128((__m128i *)grey, + _mm_or_si128(_mm_shuffle_epi8(grays_0_7, mask_low1), _mm_shuffle_epi8(grays_8_15, mask_low2))); rgb += 48; grey += 16; @@ -3569,8 +3460,7 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, } for (; i < size; i++) { - *grey = (unsigned char)(0.2126 * (*rgb) + 0.7152 * (*(rgb + 1)) + - 0.0722 * (*(rgb + 2))); + *grey = (unsigned char)(0.2126 * (*rgb) + 0.7152 * (*(rgb + 1)) + 0.0722 * (*(rgb + 2))); rgb += 3; ++grey; @@ -3582,9 +3472,7 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, 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_output = (unsigned char)(0.2126 * (*pt_input) + 0.7152 * (*(pt_input + 1)) + 0.0722 * (*(pt_input + 2))); pt_input += 3; pt_output++; } @@ -3597,8 +3485,7 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, http://www.poynton.com/notes/colour_and_gamma/ColorFAQ.html */ -void vpImageConvert::RGBaToGrey(unsigned char *rgba, unsigned char *grey, - unsigned int size) +void vpImageConvert::RGBaToGrey(unsigned char *rgba, unsigned char *grey, unsigned int size) { bool checkSSSE3 = vpCPUFeatures::checkSSSE3(); #if !VISP_HAVE_SSSE3 @@ -3611,82 +3498,54 @@ void vpImageConvert::RGBaToGrey(unsigned char *rgba, unsigned char *grey, if (size >= 16) { // Mask to select R component - const __m128i mask_R1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, 12, - -1, 8, -1, 4, -1, 0, -1); - const __m128i mask_R2 = _mm_set_epi8(12, -1, 8, -1, 4, -1, 0, -1, -1, - -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_R1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, 12, -1, 8, -1, 4, -1, 0, -1); + const __m128i mask_R2 = _mm_set_epi8(12, -1, 8, -1, 4, -1, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1); // Mask to select G component - const __m128i mask_G1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, 13, - -1, 9, -1, 5, -1, 1, -1); - const __m128i mask_G2 = _mm_set_epi8(13, -1, 9, -1, 5, -1, 1, -1, -1, - -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_G1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, 13, -1, 9, -1, 5, -1, 1, -1); + const __m128i mask_G2 = _mm_set_epi8(13, -1, 9, -1, 5, -1, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1); // Mask to select B component - const __m128i mask_B1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, 14, - -1, 10, -1, 6, -1, 2, -1); - const __m128i mask_B2 = _mm_set_epi8(14, -1, 10, -1, 6, -1, 2, -1, -1, - -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_B1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, 14, -1, 10, -1, 6, -1, 2, -1); + const __m128i mask_B2 = _mm_set_epi8(14, -1, 10, -1, 6, -1, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1); // Mask to select the gray component - const __m128i mask_low1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, - 15, 13, 11, 9, 7, 5, 3, 1); - const __m128i mask_low2 = _mm_set_epi8(15, 13, 11, 9, 7, 5, 3, 1, -1, - -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_low1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, 15, 13, 11, 9, 7, 5, 3, 1); + const __m128i mask_low2 = _mm_set_epi8(15, 13, 11, 9, 7, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1, -1); // Coefficients RGB to Gray - const __m128i coeff_R = _mm_set_epi16(13933, 13933, 13933, 13933, 13933, - 13933, 13933, 13933); - const __m128i coeff_G = - _mm_set_epi16((short int)46871, (short int)46871, (short int)46871, - (short int)46871, (short int)46871, (short int)46871, - (short int)46871, (short int)46871); - const __m128i coeff_B = - _mm_set_epi16(4732, 4732, 4732, 4732, 4732, 4732, 4732, 4732); + const __m128i coeff_R = _mm_set_epi16(13933, 13933, 13933, 13933, 13933, 13933, 13933, 13933); + const __m128i coeff_G = _mm_set_epi16((short int)46871, (short int)46871, (short int)46871, (short int)46871, + (short int)46871, (short int)46871, (short int)46871, (short int)46871); + const __m128i coeff_B = _mm_set_epi16(4732, 4732, 4732, 4732, 4732, 4732, 4732, 4732); for (; i <= size - 16; i += 16) { // Process 2*4 color pixels const __m128i data1 = _mm_loadu_si128((const __m128i *)rgba); const __m128i data2 = _mm_loadu_si128((const __m128i *)(rgba + 16)); - const __m128i red_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_R1), - _mm_shuffle_epi8(data2, mask_R2)); - const __m128i green_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_G1), - _mm_shuffle_epi8(data2, mask_G2)); - const __m128i blue_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_B1), - _mm_shuffle_epi8(data2, mask_B2)); - - const __m128i grays_0_7 = _mm_adds_epu16( - _mm_mulhi_epu16(red_0_7, coeff_R), - _mm_adds_epu16(_mm_mulhi_epu16(green_0_7, coeff_G), - _mm_mulhi_epu16(blue_0_7, coeff_B))); + const __m128i red_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_R1), _mm_shuffle_epi8(data2, mask_R2)); + const __m128i green_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_G1), _mm_shuffle_epi8(data2, mask_G2)); + const __m128i blue_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_B1), _mm_shuffle_epi8(data2, mask_B2)); + + const __m128i grays_0_7 = + _mm_adds_epu16(_mm_mulhi_epu16(red_0_7, coeff_R), + _mm_adds_epu16(_mm_mulhi_epu16(green_0_7, coeff_G), _mm_mulhi_epu16(blue_0_7, coeff_B))); // Process next 2*4 color pixels const __m128i data3 = _mm_loadu_si128((const __m128i *)(rgba + 32)); const __m128i data4 = _mm_loadu_si128((const __m128i *)(rgba + 48)); - const __m128i red_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data3, mask_R1), - _mm_shuffle_epi8(data4, mask_R2)); - const __m128i green_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data3, mask_G1), - _mm_shuffle_epi8(data4, mask_G2)); - const __m128i blue_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data3, mask_B1), - _mm_shuffle_epi8(data4, mask_B2)); - - const __m128i grays_8_15 = _mm_adds_epu16( - _mm_mulhi_epu16(red_8_15, coeff_R), - _mm_adds_epu16(_mm_mulhi_epu16(green_8_15, coeff_G), - _mm_mulhi_epu16(blue_8_15, coeff_B))); - - _mm_storeu_si128( - (__m128i *)grey, - _mm_or_si128(_mm_shuffle_epi8(grays_0_7, mask_low1), - _mm_shuffle_epi8(grays_8_15, mask_low2))); + const __m128i red_8_15 = _mm_or_si128(_mm_shuffle_epi8(data3, mask_R1), _mm_shuffle_epi8(data4, mask_R2)); + const __m128i green_8_15 = _mm_or_si128(_mm_shuffle_epi8(data3, mask_G1), _mm_shuffle_epi8(data4, mask_G2)); + const __m128i blue_8_15 = _mm_or_si128(_mm_shuffle_epi8(data3, mask_B1), _mm_shuffle_epi8(data4, mask_B2)); + + const __m128i grays_8_15 = + _mm_adds_epu16(_mm_mulhi_epu16(red_8_15, coeff_R), + _mm_adds_epu16(_mm_mulhi_epu16(green_8_15, coeff_G), _mm_mulhi_epu16(blue_8_15, coeff_B))); + + _mm_storeu_si128((__m128i *)grey, + _mm_or_si128(_mm_shuffle_epi8(grays_0_7, mask_low1), _mm_shuffle_epi8(grays_8_15, mask_low2))); rgba += 64; grey += 16; @@ -3694,8 +3553,7 @@ void vpImageConvert::RGBaToGrey(unsigned char *rgba, unsigned char *grey, } for (; i < size; i++) { - *grey = (unsigned char)(0.2126 * (*rgba) + 0.7152 * (*(rgba + 1)) + - 0.0722 * (*(rgba + 2))); + *grey = (unsigned char)(0.2126 * (*rgba) + 0.7152 * (*(rgba + 1)) + 0.0722 * (*(rgba + 2))); rgba += 4; ++grey; @@ -3707,9 +3565,7 @@ void vpImageConvert::RGBaToGrey(unsigned char *rgba, unsigned char *grey, 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_output = (unsigned char)(0.2126 * (*pt_input) + 0.7152 * (*(pt_input + 1)) + 0.0722 * (*(pt_input + 2))); pt_input += 4; pt_output++; } @@ -3721,8 +3577,7 @@ void vpImageConvert::RGBaToGrey(unsigned char *rgba, unsigned char *grey, The alpha component is set to vpRGBa::alpha_default. */ -void vpImageConvert::GreyToRGBa(unsigned char *grey, unsigned char *rgba, - unsigned int size) +void vpImageConvert::GreyToRGBa(unsigned char *grey, unsigned char *rgba, unsigned int size) { unsigned char *pt_input = grey; unsigned char *pt_end = grey + size; @@ -3744,8 +3599,7 @@ void vpImageConvert::GreyToRGBa(unsigned char *grey, unsigned char *rgba, Convert from grey image to linear RGB image. */ -void vpImageConvert::GreyToRGB(unsigned char *grey, unsigned char *rgb, - unsigned int size) +void vpImageConvert::GreyToRGB(unsigned char *grey, unsigned char *rgb, unsigned int size) { unsigned char *pt_input = grey; unsigned char *pt_end = grey + size; @@ -3769,8 +3623,7 @@ void vpImageConvert::GreyToRGB(unsigned char *grey, unsigned char *rgb, 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, +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 @@ -3800,8 +3653,7 @@ void vpImageConvert::BGRToRGBa(unsigned char *bgr, unsigned char *rgba, 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, +void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned int width, unsigned int height, bool flip) { bool checkSSSE3 = vpCPUFeatures::checkSSSE3(); @@ -3812,40 +3664,26 @@ void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, if (checkSSSE3) { #if VISP_HAVE_SSSE3 // Mask to select B component - const __m128i mask_B1 = _mm_set_epi8(-1, -1, -1, -1, 15, -1, 12, -1, 9, - -1, 6, -1, 3, -1, 0, -1); - const __m128i mask_B2 = _mm_set_epi8(5, -1, 2, -1, -1, -1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1); - const __m128i mask_B3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, - -1, 14, -1, 11, -1, 8, -1); - const __m128i mask_B4 = _mm_set_epi8(13, -1, 10, -1, 7, -1, 4, -1, 1, -1, - -1, -1, -1, -1, -1, -1); + const __m128i mask_B1 = _mm_set_epi8(-1, -1, -1, -1, 15, -1, 12, -1, 9, -1, 6, -1, 3, -1, 0, -1); + const __m128i mask_B2 = _mm_set_epi8(5, -1, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_B3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 14, -1, 11, -1, 8, -1); + const __m128i mask_B4 = _mm_set_epi8(13, -1, 10, -1, 7, -1, 4, -1, 1, -1, -1, -1, -1, -1, -1, -1); // Mask to select G component - const __m128i mask_G1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, 13, -1, 10, - -1, 7, -1, 4, -1, 1, -1); - const __m128i mask_G2 = _mm_set_epi8(6, -1, 3, -1, 0, -1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1); - const __m128i mask_G3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, - -1, 15, -1, 12, -1, 9, -1); - const __m128i mask_G4 = _mm_set_epi8(14, -1, 11, -1, 8, -1, 5, -1, 2, -1, - -1, -1, -1, -1, -1, -1); + const __m128i mask_G1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, 13, -1, 10, -1, 7, -1, 4, -1, 1, -1); + const __m128i mask_G2 = _mm_set_epi8(6, -1, 3, -1, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_G3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 15, -1, 12, -1, 9, -1); + const __m128i mask_G4 = _mm_set_epi8(14, -1, 11, -1, 8, -1, 5, -1, 2, -1, -1, -1, -1, -1, -1, -1); // Mask to select R component - const __m128i mask_R1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, 14, -1, 11, - -1, 8, -1, 5, -1, 2, -1); - const __m128i mask_R2 = _mm_set_epi8(7, -1, 4, -1, 1, -1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1); - const __m128i mask_R3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, - -1, -1, -1, 13, -1, 10, -1); - const __m128i mask_R4 = _mm_set_epi8(15, -1, 12, -1, 9, -1, 6, -1, 3, -1, - 0, -1, -1, -1, -1, -1); + const __m128i mask_R1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, 14, -1, 11, -1, 8, -1, 5, -1, 2, -1); + const __m128i mask_R2 = _mm_set_epi8(7, -1, 4, -1, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_R3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 13, -1, 10, -1); + const __m128i mask_R4 = _mm_set_epi8(15, -1, 12, -1, 9, -1, 6, -1, 3, -1, 0, -1, -1, -1, -1, -1); // Mask to select the gray component - const __m128i mask_low1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, 15, - 13, 11, 9, 7, 5, 3, 1); - const __m128i mask_low2 = _mm_set_epi8(15, 13, 11, 9, 7, 5, 3, 1, -1, -1, - -1, -1, -1, -1, -1, -1); + const __m128i mask_low1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, 15, 13, 11, 9, 7, 5, 3, 1); + const __m128i mask_low2 = _mm_set_epi8(15, 13, 11, 9, 7, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1, -1); // Coefficients RGB to Gray // const __m128i coeff_R = _mm_set_epi8( @@ -3868,14 +3706,10 @@ void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, // const __m128i coeff_B = _mm_set_epi16( // 2365*2, 2365*2, 2365*2, 2365*2, 2365*2, 2365*2, 2365*2, 2365*2 // ); - const __m128i coeff_R = - _mm_set_epi16(13933, 13933, 13933, 13933, 13933, 13933, 13933, 13933); - const __m128i coeff_G = - _mm_set_epi16((short int)46871, (short int)46871, (short int)46871, - (short int)46871, (short int)46871, (short int)46871, - (short int)46871, (short int)46871); - const __m128i coeff_B = - _mm_set_epi16(4732, 4732, 4732, 4732, 4732, 4732, 4732, 4732); + const __m128i coeff_R = _mm_set_epi16(13933, 13933, 13933, 13933, 13933, 13933, 13933, 13933); + const __m128i coeff_G = _mm_set_epi16((short int)46871, (short int)46871, (short int)46871, (short int)46871, + (short int)46871, (short int)46871, (short int)46871, (short int)46871); + const __m128i coeff_B = _mm_set_epi16(4732, 4732, 4732, 4732, 4732, 4732, 4732, 4732); if (flip) { int i = ((int)height) - 1; @@ -3892,45 +3726,27 @@ void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, for (; j <= width - 16; j += 16) { // Process 16 color pixels const __m128i data1 = _mm_loadu_si128((const __m128i *)bgr); - const __m128i data2 = - _mm_loadu_si128((const __m128i *)(bgr + 16)); - const __m128i data3 = - _mm_loadu_si128((const __m128i *)(bgr + 32)); - - const __m128i red_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_R1), - _mm_shuffle_epi8(data2, mask_R2)); - const __m128i green_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_G1), - _mm_shuffle_epi8(data2, mask_G2)); - const __m128i blue_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_B1), - _mm_shuffle_epi8(data2, mask_B2)); - - const __m128i grays_0_7 = _mm_adds_epu16( - _mm_mulhi_epu16(red_0_7, coeff_R), - _mm_adds_epu16(_mm_mulhi_epu16(green_0_7, coeff_G), - _mm_mulhi_epu16(blue_0_7, coeff_B))); - - const __m128i red_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data2, mask_R3), - _mm_shuffle_epi8(data3, mask_R4)); - const __m128i green_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data2, mask_G3), - _mm_shuffle_epi8(data3, mask_G4)); - const __m128i blue_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data2, mask_B3), - _mm_shuffle_epi8(data3, mask_B4)); - - const __m128i grays_8_15 = _mm_adds_epu16( - _mm_mulhi_epu16(red_8_15, coeff_R), - _mm_adds_epu16(_mm_mulhi_epu16(green_8_15, coeff_G), - _mm_mulhi_epu16(blue_8_15, coeff_B))); - - _mm_storeu_si128( - (__m128i *)grey, - _mm_or_si128(_mm_shuffle_epi8(grays_0_7, mask_low1), - _mm_shuffle_epi8(grays_8_15, mask_low2))); + const __m128i data2 = _mm_loadu_si128((const __m128i *)(bgr + 16)); + const __m128i data3 = _mm_loadu_si128((const __m128i *)(bgr + 32)); + + const __m128i red_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_R1), _mm_shuffle_epi8(data2, mask_R2)); + const __m128i green_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_G1), _mm_shuffle_epi8(data2, mask_G2)); + const __m128i blue_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_B1), _mm_shuffle_epi8(data2, mask_B2)); + + const __m128i grays_0_7 = + _mm_adds_epu16(_mm_mulhi_epu16(red_0_7, coeff_R), + _mm_adds_epu16(_mm_mulhi_epu16(green_0_7, coeff_G), _mm_mulhi_epu16(blue_0_7, coeff_B))); + + const __m128i red_8_15 = _mm_or_si128(_mm_shuffle_epi8(data2, mask_R3), _mm_shuffle_epi8(data3, mask_R4)); + const __m128i green_8_15 = _mm_or_si128(_mm_shuffle_epi8(data2, mask_G3), _mm_shuffle_epi8(data3, mask_G4)); + const __m128i blue_8_15 = _mm_or_si128(_mm_shuffle_epi8(data2, mask_B3), _mm_shuffle_epi8(data3, mask_B4)); + + const __m128i grays_8_15 = + _mm_adds_epu16(_mm_mulhi_epu16(red_8_15, coeff_R), _mm_adds_epu16(_mm_mulhi_epu16(green_8_15, coeff_G), + _mm_mulhi_epu16(blue_8_15, coeff_B))); + + _mm_storeu_si128((__m128i *)grey, _mm_or_si128(_mm_shuffle_epi8(grays_0_7, mask_low1), + _mm_shuffle_epi8(grays_8_15, mask_low2))); bgr += 48; grey += 16; @@ -3970,40 +3786,24 @@ void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, const __m128i data2 = _mm_loadu_si128((const __m128i *)(bgr + 16)); const __m128i data3 = _mm_loadu_si128((const __m128i *)(bgr + 32)); - const __m128i red_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_R1), - _mm_shuffle_epi8(data2, mask_R2)); - const __m128i green_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_G1), - _mm_shuffle_epi8(data2, mask_G2)); - const __m128i blue_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_B1), - _mm_shuffle_epi8(data2, mask_B2)); - - const __m128i grays_0_7 = _mm_adds_epu16( - _mm_mulhi_epu16(red_0_7, coeff_R), - _mm_adds_epu16(_mm_mulhi_epu16(green_0_7, coeff_G), - _mm_mulhi_epu16(blue_0_7, coeff_B))); - - const __m128i red_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data2, mask_R3), - _mm_shuffle_epi8(data3, mask_R4)); - const __m128i green_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data2, mask_G3), - _mm_shuffle_epi8(data3, mask_G4)); - const __m128i blue_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data2, mask_B3), - _mm_shuffle_epi8(data3, mask_B4)); - - const __m128i grays_8_15 = _mm_adds_epu16( - _mm_mulhi_epu16(red_8_15, coeff_R), - _mm_adds_epu16(_mm_mulhi_epu16(green_8_15, coeff_G), - _mm_mulhi_epu16(blue_8_15, coeff_B))); - - _mm_storeu_si128( - (__m128i *)grey, - _mm_or_si128(_mm_shuffle_epi8(grays_0_7, mask_low1), - _mm_shuffle_epi8(grays_8_15, mask_low2))); + const __m128i red_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_R1), _mm_shuffle_epi8(data2, mask_R2)); + const __m128i green_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_G1), _mm_shuffle_epi8(data2, mask_G2)); + const __m128i blue_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_B1), _mm_shuffle_epi8(data2, mask_B2)); + + const __m128i grays_0_7 = + _mm_adds_epu16(_mm_mulhi_epu16(red_0_7, coeff_R), + _mm_adds_epu16(_mm_mulhi_epu16(green_0_7, coeff_G), _mm_mulhi_epu16(blue_0_7, coeff_B))); + + const __m128i red_8_15 = _mm_or_si128(_mm_shuffle_epi8(data2, mask_R3), _mm_shuffle_epi8(data3, mask_R4)); + const __m128i green_8_15 = _mm_or_si128(_mm_shuffle_epi8(data2, mask_G3), _mm_shuffle_epi8(data3, mask_G4)); + const __m128i blue_8_15 = _mm_or_si128(_mm_shuffle_epi8(data2, mask_B3), _mm_shuffle_epi8(data3, mask_B4)); + + const __m128i grays_8_15 = + _mm_adds_epu16(_mm_mulhi_epu16(red_8_15, coeff_R), + _mm_adds_epu16(_mm_mulhi_epu16(green_8_15, coeff_G), _mm_mulhi_epu16(blue_8_15, coeff_B))); + + _mm_storeu_si128((__m128i *)grey, _mm_or_si128(_mm_shuffle_epi8(grays_0_7, mask_low1), + _mm_shuffle_epi8(grays_8_15, mask_low2))); bgr += 48; grey += 16; @@ -4011,8 +3811,7 @@ void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, } for (; i < size; i++) { - *grey = (unsigned char)(0.2126 * (*(bgr + 2)) + - 0.7152 * (*(bgr + 1)) + 0.0722 * (*bgr)); + *grey = (unsigned char)(0.2126 * (*(bgr + 2)) + 0.7152 * (*(bgr + 1)) + 0.0722 * (*bgr)); bgr += 3; ++grey; @@ -4030,9 +3829,7 @@ void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, for (unsigned int i = 0; i < height; i++) { unsigned char *line = src; for (unsigned int j = 0; j < width; j++) { - *grey++ = - (unsigned char)(0.2126 * *(line + 2) + 0.7152 * *(line + 1) + - 0.0722 * *(line + 0)); + *grey++ = (unsigned char)(0.2126 * *(line + 2) + 0.7152 * *(line + 1) + 0.0722 * *(line + 0)); line += 3; } @@ -4049,8 +3846,7 @@ void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, 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, +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 @@ -4081,8 +3877,7 @@ void vpImageConvert::RGBToRGBa(unsigned char *rgb, unsigned char *rgba, 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, +void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int width, unsigned int height, bool flip) { if (flip) { @@ -4102,50 +3897,32 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, if (width >= 16) { // Mask to select R component - const __m128i mask_R1 = _mm_set_epi8(-1, -1, -1, -1, 15, -1, 12, -1, - 9, -1, 6, -1, 3, -1, 0, -1); - const __m128i mask_R2 = _mm_set_epi8(5, -1, 2, -1, -1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1, -1); - const __m128i mask_R3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, - -1, -1, 14, -1, 11, -1, 8, -1); - const __m128i mask_R4 = _mm_set_epi8(13, -1, 10, -1, 7, -1, 4, -1, 1, - -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_R1 = _mm_set_epi8(-1, -1, -1, -1, 15, -1, 12, -1, 9, -1, 6, -1, 3, -1, 0, -1); + const __m128i mask_R2 = _mm_set_epi8(5, -1, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_R3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 14, -1, 11, -1, 8, -1); + const __m128i mask_R4 = _mm_set_epi8(13, -1, 10, -1, 7, -1, 4, -1, 1, -1, -1, -1, -1, -1, -1, -1); // Mask to select G component - const __m128i mask_G1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, 13, -1, - 10, -1, 7, -1, 4, -1, 1, -1); - const __m128i mask_G2 = _mm_set_epi8(6, -1, 3, -1, 0, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1, -1); - const __m128i mask_G3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, - -1, -1, 15, -1, 12, -1, 9, -1); - const __m128i mask_G4 = _mm_set_epi8(14, -1, 11, -1, 8, -1, 5, -1, 2, - -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_G1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, 13, -1, 10, -1, 7, -1, 4, -1, 1, -1); + const __m128i mask_G2 = _mm_set_epi8(6, -1, 3, -1, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_G3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 15, -1, 12, -1, 9, -1); + const __m128i mask_G4 = _mm_set_epi8(14, -1, 11, -1, 8, -1, 5, -1, 2, -1, -1, -1, -1, -1, -1, -1); // Mask to select B component - const __m128i mask_B1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, 14, -1, - 11, -1, 8, -1, 5, -1, 2, -1); - const __m128i mask_B2 = _mm_set_epi8(7, -1, 4, -1, 1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1, -1); - const __m128i mask_B3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, - -1, -1, -1, -1, 13, -1, 10, -1); - const __m128i mask_B4 = _mm_set_epi8(15, -1, 12, -1, 9, -1, 6, -1, 3, - -1, 0, -1, -1, -1, -1, -1); + const __m128i mask_B1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, 14, -1, 11, -1, 8, -1, 5, -1, 2, -1); + const __m128i mask_B2 = _mm_set_epi8(7, -1, 4, -1, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_B3 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 13, -1, 10, -1); + const __m128i mask_B4 = _mm_set_epi8(15, -1, 12, -1, 9, -1, 6, -1, 3, -1, 0, -1, -1, -1, -1, -1); // Mask to select the gray component - const __m128i mask_low1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, - 15, 13, 11, 9, 7, 5, 3, 1); - const __m128i mask_low2 = _mm_set_epi8(15, 13, 11, 9, 7, 5, 3, 1, -1, - -1, -1, -1, -1, -1, -1, -1); + const __m128i mask_low1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, -1, 15, 13, 11, 9, 7, 5, 3, 1); + const __m128i mask_low2 = _mm_set_epi8(15, 13, 11, 9, 7, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1, -1); // Coefficients RGB to Gray - const __m128i coeff_R = _mm_set_epi16(13933, 13933, 13933, 13933, - 13933, 13933, 13933, 13933); - const __m128i coeff_G = _mm_set_epi16( - (short int)46871, (short int)46871, (short int)46871, - (short int)46871, (short int)46871, (short int)46871, - (short int)46871, (short int)46871); - const __m128i coeff_B = - _mm_set_epi16(4732, 4732, 4732, 4732, 4732, 4732, 4732, 4732); + const __m128i coeff_R = _mm_set_epi16(13933, 13933, 13933, 13933, 13933, 13933, 13933, 13933); + const __m128i coeff_G = _mm_set_epi16((short int)46871, (short int)46871, (short int)46871, (short int)46871, + (short int)46871, (short int)46871, (short int)46871, (short int)46871); + const __m128i coeff_B = _mm_set_epi16(4732, 4732, 4732, 4732, 4732, 4732, 4732, 4732); for (; i >= 0; i--) { unsigned int j = 0; @@ -4153,45 +3930,27 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, for (; j <= width - 16; j += 16) { // Process 16 color pixels const __m128i data1 = _mm_loadu_si128((const __m128i *)rgb); - const __m128i data2 = - _mm_loadu_si128((const __m128i *)(rgb + 16)); - const __m128i data3 = - _mm_loadu_si128((const __m128i *)(rgb + 32)); - - const __m128i red_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_R1), - _mm_shuffle_epi8(data2, mask_R2)); - const __m128i green_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_G1), - _mm_shuffle_epi8(data2, mask_G2)); - const __m128i blue_0_7 = - _mm_or_si128(_mm_shuffle_epi8(data1, mask_B1), - _mm_shuffle_epi8(data2, mask_B2)); - - const __m128i grays_0_7 = _mm_adds_epu16( - _mm_mulhi_epu16(red_0_7, coeff_R), - _mm_adds_epu16(_mm_mulhi_epu16(green_0_7, coeff_G), - _mm_mulhi_epu16(blue_0_7, coeff_B))); - - const __m128i red_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data2, mask_R3), - _mm_shuffle_epi8(data3, mask_R4)); - const __m128i green_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data2, mask_G3), - _mm_shuffle_epi8(data3, mask_G4)); - const __m128i blue_8_15 = - _mm_or_si128(_mm_shuffle_epi8(data2, mask_B3), - _mm_shuffle_epi8(data3, mask_B4)); - - const __m128i grays_8_15 = _mm_adds_epu16( - _mm_mulhi_epu16(red_8_15, coeff_R), - _mm_adds_epu16(_mm_mulhi_epu16(green_8_15, coeff_G), - _mm_mulhi_epu16(blue_8_15, coeff_B))); - - _mm_storeu_si128( - (__m128i *)grey, - _mm_or_si128(_mm_shuffle_epi8(grays_0_7, mask_low1), - _mm_shuffle_epi8(grays_8_15, mask_low2))); + const __m128i data2 = _mm_loadu_si128((const __m128i *)(rgb + 16)); + const __m128i data3 = _mm_loadu_si128((const __m128i *)(rgb + 32)); + + const __m128i red_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_R1), _mm_shuffle_epi8(data2, mask_R2)); + const __m128i green_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_G1), _mm_shuffle_epi8(data2, mask_G2)); + const __m128i blue_0_7 = _mm_or_si128(_mm_shuffle_epi8(data1, mask_B1), _mm_shuffle_epi8(data2, mask_B2)); + + const __m128i grays_0_7 = + _mm_adds_epu16(_mm_mulhi_epu16(red_0_7, coeff_R), + _mm_adds_epu16(_mm_mulhi_epu16(green_0_7, coeff_G), _mm_mulhi_epu16(blue_0_7, coeff_B))); + + const __m128i red_8_15 = _mm_or_si128(_mm_shuffle_epi8(data2, mask_R3), _mm_shuffle_epi8(data3, mask_R4)); + const __m128i green_8_15 = _mm_or_si128(_mm_shuffle_epi8(data2, mask_G3), _mm_shuffle_epi8(data3, mask_G4)); + const __m128i blue_8_15 = _mm_or_si128(_mm_shuffle_epi8(data2, mask_B3), _mm_shuffle_epi8(data3, mask_B4)); + + const __m128i grays_8_15 = + _mm_adds_epu16(_mm_mulhi_epu16(red_8_15, coeff_R), _mm_adds_epu16(_mm_mulhi_epu16(green_8_15, coeff_G), + _mm_mulhi_epu16(blue_8_15, coeff_B))); + + _mm_storeu_si128((__m128i *)grey, _mm_or_si128(_mm_shuffle_epi8(grays_0_7, mask_low1), + _mm_shuffle_epi8(grays_8_15, mask_low2))); rgb += 48; grey += 16; @@ -4227,8 +3986,7 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, 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 *src = (flip) ? rgb + (width * height * 3) + lineStep : rgb; unsigned int j = 0; unsigned int i = 0; @@ -4293,8 +4051,7 @@ void vpImageConvert::computeYCbCrLUT() */ -void vpImageConvert::YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, - unsigned int size) +void vpImageConvert::YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, unsigned int size) { unsigned char *cbv; unsigned char *crv; @@ -4315,25 +4072,14 @@ void vpImageConvert::YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, } val_r = *pt_ycbcr + vpImageConvert::vpCrr[*crv]; - val_g = - *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*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_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; } @@ -4362,8 +4108,7 @@ void vpImageConvert::YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, */ -void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, - unsigned int size) +void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, unsigned int size) { unsigned char *cbv; unsigned char *crv; @@ -4384,26 +4129,14 @@ void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, } val_r = *pt_ycbcr + vpImageConvert::vpCrr[*crv]; - val_g = - *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*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++ = (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++ = vpRGBa::alpha_default; pt_ycbcr += 2; @@ -4426,8 +4159,7 @@ void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, - In grey format, each pixel is coded using 8 bytes. */ -void vpImageConvert::YCbCrToGrey(unsigned char *yuv, unsigned char *grey, - unsigned int size) +void vpImageConvert::YCbCrToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) { unsigned int i = 0, j = 0; @@ -4456,8 +4188,7 @@ void vpImageConvert::YCbCrToGrey(unsigned char *yuv, unsigned char *grey, Byte 2: Blue */ -void vpImageConvert::YCrCbToRGB(unsigned char *ycrcb, unsigned char *rgb, - unsigned int size) +void vpImageConvert::YCrCbToRGB(unsigned char *ycrcb, unsigned char *rgb, unsigned int size) { unsigned char *cbv; unsigned char *crv; @@ -4478,25 +4209,14 @@ void vpImageConvert::YCrCbToRGB(unsigned char *ycrcb, unsigned char *rgb, } val_r = *pt_ycbcr + vpImageConvert::vpCrr[*crv]; - val_g = - *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*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_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; } @@ -4523,8 +4243,7 @@ void vpImageConvert::YCrCbToRGB(unsigned char *ycrcb, unsigned char *rgb, */ -void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, - unsigned int size) +void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, unsigned int size) { unsigned char *cbv; unsigned char *crv; @@ -4545,26 +4264,14 @@ void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, } val_r = *pt_ycbcr + vpImageConvert::vpCrr[*crv]; - val_g = - *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*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++ = (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++ = vpRGBa::alpha_default; pt_ycbcr += 2; @@ -4606,11 +4313,8 @@ int main() } \endcode */ -void vpImageConvert::split(const vpImage &src, - vpImage *pR, - vpImage *pG, - vpImage *pB, - vpImage *pa) +void vpImageConvert::split(const vpImage &src, vpImage *pR, vpImage *pG, + vpImage *pB, vpImage *pa) { size_t n = src.getNumberOfPixel(); unsigned int height = src.getHeight(); @@ -4634,8 +4338,7 @@ void vpImageConvert::split(const vpImage &src, 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) { + if (tabChannel[j]->getHeight() != height || tabChannel[j]->getWidth() != width) { tabChannel[j]->resize(height, width); } dst = (unsigned char *)tabChannel[j]->bitmap; @@ -4679,11 +4382,8 @@ void vpImageConvert::split(const vpImage &src, \param a : Alpha channel. \param RGBa : Destination RGBa image. */ -void vpImageConvert::merge(const vpImage *R, - const vpImage *G, - const vpImage *B, - const vpImage *a, - vpImage &RGBa) +void vpImageConvert::merge(const vpImage *R, const vpImage *G, + const vpImage *B, const vpImage *a, vpImage &RGBa) { // Check if the input channels have all the same dimensions std::map mapOfWidths, mapOfHeights; @@ -4746,8 +4446,7 @@ void vpImageConvert::merge(const vpImage *R, \param size : The image size or the number of pixels. */ -void vpImageConvert::MONO16ToGrey(unsigned char *grey16, unsigned char *grey, - unsigned int size) +void vpImageConvert::MONO16ToGrey(unsigned char *grey16, unsigned char *grey, unsigned int size) { int i = (((int)size) << 1) - 1; int j = (int)size - 1; @@ -4770,16 +4469,14 @@ void vpImageConvert::MONO16ToGrey(unsigned char *grey16, unsigned char *grey, \param size : The image size or the number of pixels. */ -void vpImageConvert::MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba, - unsigned int size) +void vpImageConvert::MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba, unsigned int size) { int i = (((int)size) << 1) - 1; int j = (int)(size * 4 - 1); while (i >= 0) { int y = grey16[i--]; - unsigned char v = - static_cast((y + (grey16[i--] << 8)) >> 8); + unsigned char v = static_cast((y + (grey16[i--] << 8)) >> 8); rgba[j--] = vpRGBa::alpha_default; rgba[j--] = v; rgba[j--] = v; @@ -4787,15 +4484,13 @@ void vpImageConvert::MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba, } } -void vpImageConvert::HSV2RGB(const double *hue_, const double *saturation_, - const double *value_, unsigned char *rgb, +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 hue = hue_[i], saturation = saturation_[i], value = value_[i]; - if (vpMath::equal(saturation, 0.0, - std::numeric_limits::epsilon())) { + if (vpMath::equal(saturation, 0.0, std::numeric_limits::epsilon())) { hue = value; saturation = value; } else { @@ -4859,8 +4554,7 @@ void vpImageConvert::HSV2RGB(const double *hue_, const double *saturation_, } } -void vpImageConvert::RGB2HSV(const unsigned char *rgb, double *hue, - double *saturation, double *value, +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++) { @@ -4898,8 +4592,7 @@ void vpImageConvert::RGB2HSV(const unsigned char *rgb, double *hue, if (vpMath::equal(red, max, std::numeric_limits::epsilon())) { h = (green - blue) / delta; - } else if (vpMath::equal(green, max, - std::numeric_limits::epsilon())) { + } else if (vpMath::equal(green, max, std::numeric_limits::epsilon())) { h = 2 + (blue - red) / delta; } else { h = 4 + (red - green) / delta; @@ -4931,8 +4624,7 @@ void vpImageConvert::RGB2HSV(const unsigned char *rgb, double *hue, 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, +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); @@ -4950,14 +4642,11 @@ void vpImageConvert::HSVToRGBa(const double *hue, const double *saturation, 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, +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; + 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); } @@ -4974,8 +4663,7 @@ void vpImageConvert::HSVToRGBa(const unsigned char *hue, 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, +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); @@ -4992,8 +4680,7 @@ void vpImageConvert::RGBaToHSV(const unsigned char *rgba, double *hue, 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, +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++) { @@ -5015,8 +4702,7 @@ void vpImageConvert::RGBaToHSV(const unsigned char *rgba, unsigned char *hue, \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, +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); @@ -5031,14 +4717,11 @@ void vpImageConvert::HSVToRGB(const double *hue, const double *saturation, \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) +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; + 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); } @@ -5054,8 +4737,7 @@ void vpImageConvert::HSVToRGB(const unsigned char *hue, 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, +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); @@ -5071,9 +4753,8 @@ void vpImageConvert::RGBToHSV(const unsigned char *rgb, double *hue, 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) +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; diff --git a/modules/core/src/image/vpImageFilter.cpp b/modules/core/src/image/vpImageFilter.cpp index 25afcac5e9..df63b716a6 100644 --- a/modules/core/src/image/vpImageFilter.cpp +++ b/modules/core/src/image/vpImageFilter.cpp @@ -73,9 +73,7 @@ \f] Only pixels in the input image fully covered by the kernel are considered. */ -void vpImageFilter::filter(const vpImage &I, - vpImage &If, const vpMatrix &M, - const bool convolve) +void vpImageFilter::filter(const vpImage &I, vpImage &If, const vpMatrix &M, const bool convolve) { unsigned int size_y = M.getRows(), size_x = M.getCols(); unsigned int half_size_y = size_y / 2, half_size_x = size_x / 2; @@ -84,14 +82,12 @@ void vpImageFilter::filter(const vpImage &I, if (convolve) { for (unsigned int i = half_size_y; i < I.getHeight() - half_size_y; i++) { - for (unsigned int j = half_size_x; j < I.getWidth() - half_size_x; - j++) { + for (unsigned int j = half_size_x; j < I.getWidth() - half_size_x; j++) { double conv = 0; for (unsigned int a = 0; a < size_y; a++) { for (unsigned int b = 0; b < size_x; b++) { - double val = - I[i + half_size_y - a][j + half_size_x - b]; // Convolution + double val = I[i + half_size_y - a][j + half_size_x - b]; // Convolution conv += M[a][b] * val; } } @@ -100,14 +96,12 @@ void vpImageFilter::filter(const vpImage &I, } } else { for (unsigned int i = half_size_y; i < I.getHeight() - half_size_y; i++) { - for (unsigned int j = half_size_x; j < I.getWidth() - half_size_x; - j++) { + for (unsigned int j = half_size_x; j < I.getWidth() - half_size_x; j++) { double corr = 0; for (unsigned int a = 0; a < size_y; a++) { for (unsigned int b = 0; b < size_x; b++) { - double val = - I[i - half_size_y + a][j - half_size_x + b]; // Correlation + double val = I[i - half_size_y + a][j - half_size_x + b]; // Correlation corr += M[a][b] * val; } } @@ -129,8 +123,7 @@ void vpImageFilter::filter(const vpImage &I, \param M : Filter kernel. \param convolve : If true, perform a convolution otherwise a correlation. */ -void vpImageFilter::filter(const vpImage &I, vpImage &Iu, - vpImage &Iv, const vpMatrix &M, +void vpImageFilter::filter(const vpImage &I, vpImage &Iu, vpImage &Iv, const vpMatrix &M, const bool convolve) { unsigned int size = M.getRows(); @@ -147,8 +140,7 @@ void vpImageFilter::filter(const vpImage &I, vpImage &Iu, 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]; // Convolution + double val = I[v + half_size - a][u + half_size - b]; // Convolution conv_u += M[a][b] * val; conv_v += M[b][a] * val; } @@ -165,8 +157,7 @@ void vpImageFilter::filter(const vpImage &I, vpImage &Iu, 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]; // Correlation + double val = I[v - half_size + a][u - half_size + b]; // Correlation conv_u += M[a][b] * val; conv_v += M[b][a] * val; } @@ -230,8 +221,7 @@ void vpImageFilter::filter(const vpImage &I, vpImage &Iu, \note Only pixels in the input image fully covered by the kernel are considered. */ -void vpImageFilter::sepFilter(const vpImage &I, - vpImage &If, const vpColVector &kernelH, +void vpImageFilter::sepFilter(const vpImage &I, vpImage &If, const vpColVector &kernelH, const vpColVector &kernelV) { unsigned int size = kernelH.size(); @@ -290,8 +280,9 @@ int main() //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); + vpImageFilter::canny(Isrc, Icanny, gaussianFilterSize, thresholdCanny, apertureSobel); +#endif + return (0); } \endcode @@ -303,23 +294,18 @@ apertureSobel); #endif return (0); 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 &Isrc, - vpImage &Ires, - const unsigned int gaussianFilterSize, - const double thresholdCanny, +void vpImageFilter::canny(const vpImage &Isrc, vpImage &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); + 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); + 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); @@ -327,11 +313,8 @@ void vpImageFilter::canny(const vpImage &Isrc, #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); + 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 } @@ -340,8 +323,7 @@ void vpImageFilter::canny(const vpImage &Isrc, /*! Apply a separable filter. */ -void vpImageFilter::filter(const vpImage &I, - vpImage &GI, const double *filter, +void vpImageFilter::filter(const vpImage &I, vpImage &GI, const double *filter, unsigned int size) { vpImage GIx; @@ -353,8 +335,7 @@ void vpImageFilter::filter(const vpImage &I, /*! Apply a separable filter. */ -void vpImageFilter::filter(const vpImage &I, vpImage &GI, - const double *filter, unsigned int size) +void vpImageFilter::filter(const vpImage &I, vpImage &GI, const double *filter, unsigned int size) { vpImage GIx; filterX(I, GIx, filter, size); @@ -362,8 +343,7 @@ void vpImageFilter::filter(const vpImage &I, vpImage &GI, GIx.destroy(); } -void vpImageFilter::filterX(const vpImage &I, - vpImage &dIx, const double *filter, +void vpImageFilter::filterX(const vpImage &I, vpImage &dIx, const double *filter, unsigned int size) { dIx.resize(I.getHeight(), I.getWidth()); @@ -372,19 +352,16 @@ void vpImageFilter::filterX(const vpImage &I, 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++) { + 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++) { + 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 &I, vpImage &dIx, - const double *filter, unsigned int size) +void vpImageFilter::filterX(const vpImage &I, vpImage &dIx, const double *filter, unsigned int size) { dIx.resize(I.getHeight(), I.getWidth()); for (unsigned int i = 0; i < I.getHeight(); i++) { @@ -392,19 +369,16 @@ void vpImageFilter::filterX(const vpImage &I, vpImage &dIx, 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++) { + 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++) { + 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 &I, - vpImage &dIy, const double *filter, +void vpImageFilter::filterY(const vpImage &I, vpImage &dIy, const double *filter, unsigned int size) { dIy.resize(I.getHeight(), I.getWidth()); @@ -413,21 +387,18 @@ void vpImageFilter::filterY(const vpImage &I, 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 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 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 &I, vpImage &dIy, - const double *filter, unsigned int size) +void vpImageFilter::filterY(const vpImage &I, vpImage &dIy, const double *filter, unsigned int size) { dIy.resize(I.getHeight(), I.getWidth()); for (unsigned int i = 0; i < (size - 1) / 2; i++) { @@ -435,14 +406,12 @@ void vpImageFilter::filterY(const vpImage &I, vpImage &dIy, 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 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 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); } @@ -460,9 +429,8 @@ void vpImageFilter::filterY(const vpImage &I, vpImage &dIy, not. */ -void vpImageFilter::gaussianBlur(const vpImage &I, - vpImage &GI, unsigned int size, - double sigma, bool normalize) +void vpImageFilter::gaussianBlur(const vpImage &I, vpImage &GI, unsigned int size, double sigma, + bool normalize) { double *fg = new double[(size + 1) / 2]; vpImageFilter::getGaussianKernel(fg, size, sigma, normalize); @@ -484,9 +452,8 @@ void vpImageFilter::gaussianBlur(const vpImage &I, not. */ -void vpImageFilter::gaussianBlur(const vpImage &I, - vpImage &GI, unsigned int size, - double sigma, bool normalize) +void vpImageFilter::gaussianBlur(const vpImage &I, vpImage &GI, unsigned int size, double sigma, + bool normalize) { double *fg = new double[(size + 1) / 2]; vpImageFilter::getGaussianKernel(fg, size, sigma, normalize); @@ -508,12 +475,10 @@ void vpImageFilter::gaussianBlur(const vpImage &I, 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) +void vpImageFilter::getGaussianKernel(double *filter, unsigned int size, double sigma, bool normalize) { if (size % 2 != 1) - throw(vpImageException(vpImageException::incorrectInitializationError, - "Bad Gaussian filter size")); + throw(vpImageException(vpImageException::incorrectInitializationError, "Bad Gaussian filter size")); if (sigma <= 0) sigma = (size - 1) / 6.0; @@ -521,8 +486,7 @@ void vpImageFilter::getGaussianKernel(double *filter, unsigned int size, 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)); + filter[i] = (1. / (sigma * sqrt(2. * M_PI))) * exp(-(i * i) / (2. * sigma2)); } if (normalize) { // renormalization @@ -550,13 +514,10 @@ void vpImageFilter::getGaussianKernel(double *filter, unsigned int size, 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) +void vpImageFilter::getGaussianDerivativeKernel(double *filter, unsigned int size, double sigma, bool normalize) { if (size % 2 != 1) - throw(vpImageException(vpImageException::incorrectInitializationError, - "Bad Gaussian filter size")); + throw(vpImageException(vpImageException::incorrectInitializationError, "Bad Gaussian filter size")); if (sigma <= 0) sigma = (size - 1) / 6.0; @@ -566,16 +527,13 @@ void vpImageFilter::getGaussianDerivativeKernel(double *filter, 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.; + (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 += 2. * (1. / (sigma * sqrt(2. * M_PI))) * exp(-(i * i) / (2. * sigma2)); } sum += (1. / (sigma * sqrt(2. * M_PI))); @@ -585,8 +543,7 @@ void vpImageFilter::getGaussianDerivativeKernel(double *filter, } } -void vpImageFilter::getGradX(const vpImage &I, - vpImage &dIx) +void vpImageFilter::getGradX(const vpImage &I, vpImage &dIx) { dIx.resize(I.getHeight(), I.getWidth()); // dIx=0; @@ -603,8 +560,7 @@ void vpImageFilter::getGradX(const vpImage &I, } } -void vpImageFilter::getGradY(const vpImage &I, - vpImage &dIy) +void vpImageFilter::getGradY(const vpImage &I, vpImage &dIy) { dIy.resize(I.getHeight(), I.getWidth()); // dIy=0; @@ -625,8 +581,7 @@ void vpImageFilter::getGradY(const vpImage &I, } } -void vpImageFilter::getGradX(const vpImage &I, - vpImage &dIx, const double *filter, +void vpImageFilter::getGradX(const vpImage &I, vpImage &dIx, const double *filter, unsigned int size) { dIx.resize(I.getHeight(), I.getWidth()); @@ -635,18 +590,15 @@ void vpImageFilter::getGradX(const vpImage &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++) { + 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++) { + for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) { dIx[i][j] = 0; } } } -void vpImageFilter::getGradX(const vpImage &I, vpImage &dIx, - const double *filter, unsigned int size) +void vpImageFilter::getGradX(const vpImage &I, vpImage &dIx, const double *filter, unsigned int size) { dIx.resize(I.getHeight(), I.getWidth()); // dIx=0; @@ -654,19 +606,16 @@ void vpImageFilter::getGradX(const vpImage &I, vpImage &dIx, 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++) { + 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++) { + for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) { dIx[i][j] = 0; } } } -void vpImageFilter::getGradY(const vpImage &I, - vpImage &dIy, const double *filter, +void vpImageFilter::getGradY(const vpImage &I, vpImage &dIy, const double *filter, unsigned int size) { dIy.resize(I.getHeight(), I.getWidth()); @@ -677,23 +626,20 @@ void vpImageFilter::getGradY(const vpImage &I, } } //#pragma omp parallel for - for (unsigned int i = (size - 1) / 2; i < I.getHeight() - (size - 1) / 2; - i++) { + 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 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 &I, vpImage &dIy, - const double *filter, unsigned int size) +void vpImageFilter::getGradY(const vpImage &I, vpImage &dIy, const double *filter, unsigned int size) { dIy.resize(I.getHeight(), I.getWidth()); // dIy=0; @@ -702,14 +648,12 @@ void vpImageFilter::getGradY(const vpImage &I, vpImage &dIy, dIy[i][j] = 0; } } - for (unsigned int i = (size - 1) / 2; i < I.getHeight() - (size - 1) / 2; - i++) { + 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 i = I.getHeight() - (size - 1) / 2; i < I.getHeight(); i++) { for (unsigned int j = 0; j < I.getWidth(); j++) { dIy[i][j] = 0; } @@ -726,11 +670,8 @@ void vpImageFilter::getGradY(const vpImage &I, vpImage &dIy, vpImageFilter::getGaussianDerivativeKernel(). \param size : Size of the Gaussian and Gaussian derivative kernels. */ -void vpImageFilter::getGradXGauss2D(const vpImage &I, - vpImage &dIx, - const double *gaussianKernel, - const double *gaussianDerivativeKernel, - unsigned int size) +void vpImageFilter::getGradXGauss2D(const vpImage &I, vpImage &dIx, const double *gaussianKernel, + const double *gaussianDerivativeKernel, unsigned int size) { vpImage GIy; vpImageFilter::filterY(I, GIy, gaussianKernel, size); @@ -747,11 +688,8 @@ void vpImageFilter::getGradXGauss2D(const vpImage &I, vpImageFilter::getGaussianDerivativeKernel(). \param size : Size of the Gaussian and Gaussian derivative kernels. */ -void vpImageFilter::getGradYGauss2D(const vpImage &I, - vpImage &dIy, - const double *gaussianKernel, - const double *gaussianDerivativeKernel, - unsigned int size) +void vpImageFilter::getGradYGauss2D(const vpImage &I, vpImage &dIy, const double *gaussianKernel, + const double *gaussianDerivativeKernel, unsigned int size) { vpImage GIx; vpImageFilter::filterX(I, GIx, gaussianKernel, size); @@ -759,31 +697,24 @@ void vpImageFilter::getGradYGauss2D(const vpImage &I, } // operation pour pyramide gaussienne -void vpImageFilter::getGaussPyramidal(const vpImage &I, - vpImage &GI) +void vpImageFilter::getGaussPyramidal(const vpImage &I, vpImage &GI) { vpImage 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)); + 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)); + 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); + 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); @@ -798,8 +729,7 @@ void vpImageFilter::getGaussPyramidal(const vpImage &I, #endif } -void vpImageFilter::getGaussXPyramidal(const vpImage &I, - vpImage &GI) +void vpImageFilter::getGaussXPyramidal(const vpImage &I, vpImage &GI) { #if 0 GI.resize(I.getHeight(),(int)((I.getWidth()+1.)/2.)) ; @@ -826,8 +756,7 @@ void vpImageFilter::getGaussXPyramidal(const vpImage &I, #endif } -void vpImageFilter::getGaussYPyramidal(const vpImage &I, - vpImage &GI) +void vpImageFilter::getGaussYPyramidal(const vpImage &I, vpImage &GI) { #ifdef ORIG @@ -837,8 +766,7 @@ void vpImageFilter::getGaussYPyramidal(const vpImage &I, 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]; + GI[(int)((I.getHeight() + 1.) / 2.) - 1][j] = I[2 * ((int)((I.getHeight() + 1.) / 2.) - 1)][j]; } #else diff --git a/modules/core/src/image/vpImageMorphology.cpp b/modules/core/src/image/vpImageMorphology.cpp index 60ef0a8e2a..531034eb72 100644 --- a/modules/core/src/image/vpImageMorphology.cpp +++ b/modules/core/src/image/vpImageMorphology.cpp @@ -39,8 +39,7 @@ #include #include -#if defined __SSE2__ || defined _M_X64 || \ - (defined _M_IX86_FP && _M_IX86_FP >= 2) +#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2) #include #define VISP_HAVE_SSE2 1 #endif @@ -68,8 +67,7 @@ \sa dilatation(vpImage &, const vpConnexityType &) */ -void vpImageMorphology::erosion(vpImage &I, - const vpConnexityType &connexity) +void vpImageMorphology::erosion(vpImage &I, const vpConnexityType &connexity) { if (I.getSize() == 0) { std::cerr << "Input image is empty!" << std::endl; @@ -93,8 +91,7 @@ void vpImageMorphology::erosion(vpImage &I, } if (connexity == CONNEXITY_4) { - unsigned int offset[5] = {1, J.getWidth(), J.getWidth() + 1, - J.getWidth() + 2, J.getWidth() * 2 + 1}; + unsigned int offset[5] = {1, J.getWidth(), J.getWidth() + 1, J.getWidth() + 2, J.getWidth() * 2 + 1}; #if VISP_HAVE_SSE2 bool checkSSE2 = vpCPUFeatures::checkSSE2(); #endif @@ -107,20 +104,11 @@ void vpImageMorphology::erosion(vpImage &I, #if VISP_HAVE_SSE2 if (checkSSE2 && I.getWidth() >= 16) { for (; j <= I.getWidth() - 16; j += 16) { - __m128i m = - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[0])); - m = _mm_min_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[1]))); - m = _mm_min_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[2]))); - m = _mm_min_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[3]))); - m = _mm_min_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[4]))); + __m128i m = _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[0])); + m = _mm_min_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[1]))); + m = _mm_min_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[2]))); + m = _mm_min_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[3]))); + m = _mm_min_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[4]))); _mm_storeu_si128((__m128i *)(ptr_curr_I + j), m); } @@ -159,32 +147,15 @@ void vpImageMorphology::erosion(vpImage &I, #if VISP_HAVE_SSE2 if (checkSSE2 && I.getWidth() >= 16) { for (; j <= I.getWidth() - 16; j += 16) { - __m128i m = - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[0])); - m = _mm_min_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[1]))); - m = _mm_min_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[2]))); - m = _mm_min_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[3]))); - m = _mm_min_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[4]))); - m = _mm_min_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[5]))); - m = _mm_min_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[6]))); - m = _mm_min_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[7]))); - m = _mm_min_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[8]))); + __m128i m = _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[0])); + m = _mm_min_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[1]))); + m = _mm_min_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[2]))); + m = _mm_min_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[3]))); + m = _mm_min_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[4]))); + m = _mm_min_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[5]))); + m = _mm_min_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[6]))); + m = _mm_min_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[7]))); + m = _mm_min_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[8]))); _mm_storeu_si128((__m128i *)(ptr_curr_I + j), m); } @@ -226,8 +197,7 @@ void vpImageMorphology::erosion(vpImage &I, \sa erosion(vpImage &, const vpConnexityType &) */ -void vpImageMorphology::dilatation(vpImage &I, - const vpConnexityType &connexity) +void vpImageMorphology::dilatation(vpImage &I, const vpConnexityType &connexity) { if (I.getSize() == 0) { std::cerr << "Input image is empty!" << std::endl; @@ -251,8 +221,7 @@ void vpImageMorphology::dilatation(vpImage &I, } if (connexity == CONNEXITY_4) { - unsigned int offset[5] = {1, J.getWidth(), J.getWidth() + 1, - J.getWidth() + 2, J.getWidth() * 2 + 1}; + unsigned int offset[5] = {1, J.getWidth(), J.getWidth() + 1, J.getWidth() + 2, J.getWidth() * 2 + 1}; #if VISP_HAVE_SSE2 bool checkSSE2 = vpCPUFeatures::checkSSE2(); #endif @@ -265,20 +234,11 @@ void vpImageMorphology::dilatation(vpImage &I, #if VISP_HAVE_SSE2 if (checkSSE2 && I.getWidth() >= 16) { for (; j <= I.getWidth() - 16; j += 16) { - __m128i m = - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[0])); - m = _mm_max_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[1]))); - m = _mm_max_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[2]))); - m = _mm_max_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[3]))); - m = _mm_max_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[4]))); + __m128i m = _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[0])); + m = _mm_max_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[1]))); + m = _mm_max_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[2]))); + m = _mm_max_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[3]))); + m = _mm_max_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[4]))); _mm_storeu_si128((__m128i *)(ptr_curr_I + j), m); } @@ -317,32 +277,15 @@ void vpImageMorphology::dilatation(vpImage &I, #if VISP_HAVE_SSE2 if (checkSSE2 && I.getWidth() >= 16) { for (; j <= I.getWidth() - 16; j += 16) { - __m128i m = - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[0])); - m = _mm_max_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[1]))); - m = _mm_max_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[2]))); - m = _mm_max_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[3]))); - m = _mm_max_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[4]))); - m = _mm_max_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[5]))); - m = _mm_max_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[6]))); - m = _mm_max_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[7]))); - m = _mm_max_epu8( - m, - _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[8]))); + __m128i m = _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[0])); + m = _mm_max_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[1]))); + m = _mm_max_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[2]))); + m = _mm_max_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[3]))); + m = _mm_max_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[4]))); + m = _mm_max_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[5]))); + m = _mm_max_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[6]))); + m = _mm_max_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[7]))); + m = _mm_max_epu8(m, _mm_loadu_si128((const __m128i *)(ptr_curr_J + j + offset[8]))); _mm_storeu_si128((__m128i *)(ptr_curr_I + j), m); } diff --git a/modules/core/src/image/vpImagePoint.cpp b/modules/core/src/image/vpImagePoint.cpp index 82a94dcd91..c1b4c1602d 100644 --- a/modules/core/src/image/vpImagePoint.cpp +++ b/modules/core/src/image/vpImagePoint.cpp @@ -52,8 +52,8 @@ */ bool vpImagePoint::inRectangle(const vpRect &rect) const { - return (this->i <= rect.getBottom() && this->i >= rect.getTop() && - this->j <= rect.getRight() && this->j >= rect.getLeft()); + return (this->i <= rect.getBottom() && this->i >= rect.getTop() && this->j <= rect.getRight() && + this->j >= rect.getLeft()); } /*! @@ -70,16 +70,16 @@ points. int main() { -std::vector ip(2); + std::vector ip(2); -ip[0].set_ij(100, 200); -ip[1].set_ij(300, 400); + ip[0].set_ij(100, 200); + ip[1].set_ij(300, 400); -vpImagePoint cog(0,0); -for(unsigned int i=0; i #include #include - +points. +\code +#include +#include +#include int main() { -std::vector ip(2); + std::vector ip(2); -ip[0].set_ij(100, 200); -ip[1].set_ij(300, 400); + ip[0].set_ij(100, 200); + ip[1].set_ij(300, 400); -vpImagePoint cog(0,0); -for(unsigned int i=0; i::epsilon()) && - (std::fabs(j1 - j2) <= std::fabs(vpMath::maximum(j1, j2)) * - std::numeric_limits::epsilon())); + return ((std::fabs(i1 - i2) <= std::fabs(vpMath::maximum(i1, i2)) * std::numeric_limits::epsilon()) && + (std::fabs(j1 - j2) <= std::fabs(vpMath::maximum(j1, j2)) * std::numeric_limits::epsilon())); } /*! @@ -161,10 +161,8 @@ VISP_EXPORT bool operator!=(const vpImagePoint &ip1, const vpImagePoint &ip2) double i2 = ip2.get_i(); double j2 = ip2.get_j(); - return ((std::fabs(i1 - i2) > std::fabs(vpMath::maximum(i1, i2)) * - std::numeric_limits::epsilon()) || - (std::fabs(j1 - j2) > std::fabs(vpMath::maximum(j1, j2)) * - std::numeric_limits::epsilon())); + return ((std::fabs(i1 - i2) > std::fabs(vpMath::maximum(i1, i2)) * std::numeric_limits::epsilon()) || + (std::fabs(j1 - j2) > std::fabs(vpMath::maximum(j1, j2)) * std::numeric_limits::epsilon())); } /*! @@ -174,8 +172,7 @@ VISP_EXPORT bool operator!=(const vpImagePoint &ip1, const vpImagePoint &ip2) 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) +VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, const vpImagePoint &ip2) { return (vpImagePoint(ip1.get_i() + ip2.get_i(), ip1.get_j() + ip2.get_j())); } @@ -186,8 +183,7 @@ VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, 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) +VISP_EXPORT vpImagePoint operator+=(const vpImagePoint &ip1, const vpImagePoint &ip2) { return (vpImagePoint(ip1.get_i() + ip2.get_i(), ip1.get_j() + ip2.get_j())); } @@ -203,8 +199,8 @@ VISP_EXPORT vpImagePoint operator+=(const vpImagePoint &ip1, 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) + 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; @@ -228,17 +224,15 @@ VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, const int offset) 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+10u: " << ip+10u << std::endl; // new coordinates (110, -210) + 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+10u: " << ip+10u << std::endl; // new coordinates (110, 210) return 0; } \endcode */ -VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, - const unsigned int offset) +VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, const unsigned int offset) { return (vpImagePoint(ip1.get_i() + offset, ip1.get_j() + offset)); } @@ -255,17 +249,15 @@ VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, 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) + 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) +VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, const double offset) { return (vpImagePoint(ip1.get_i() + offset, ip1.get_j() + offset)); } @@ -278,8 +270,7 @@ VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, ip2 \f$. */ -VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, - const vpImagePoint &ip2) +VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, const vpImagePoint &ip2) { return (vpImagePoint(ip1.get_i() - ip2.get_i(), ip1.get_j() - ip2.get_j())); } @@ -295,8 +286,8 @@ VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, 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) + 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; @@ -319,16 +310,15 @@ VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, const int offset) 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) + 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-10u: " << ip-10u << std::endl; // new coordinates (90, 190) return 0; } \endcode */ -VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, - const unsigned int offset) +VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, const unsigned int offset) { return (vpImagePoint(ip1.get_i() - offset, ip1.get_j() - offset)); } @@ -345,17 +335,15 @@ VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, 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) + 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) +VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, const double offset) { return (vpImagePoint(ip1.get_i() - offset, ip1.get_j() - offset)); } @@ -371,16 +359,15 @@ VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, 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) + 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) +VISP_EXPORT vpImagePoint operator*(const vpImagePoint &ip1, const double scale) { return (vpImagePoint(ip1.get_i() * scale, ip1.get_j() * scale)); } @@ -396,16 +383,15 @@ VISP_EXPORT vpImagePoint operator*(const vpImagePoint &ip1, 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) + 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) +VISP_EXPORT vpImagePoint operator/(const vpImagePoint &ip1, const double scale) { return (vpImagePoint(ip1.get_i() / scale, ip1.get_j() / scale)); } diff --git a/modules/core/src/image/vpImageTools.cpp b/modules/core/src/image/vpImageTools.cpp index f633415324..33bbca304b 100644 --- a/modules/core/src/image/vpImageTools.cpp +++ b/modules/core/src/image/vpImageTools.cpp @@ -39,8 +39,7 @@ #include #include -#if defined __SSE2__ || defined _M_X64 || \ - (defined _M_IX86_FP && _M_IX86_FP >= 2) +#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2) #include #define VISP_HAVE_SSE2 1 #endif @@ -96,21 +95,18 @@ int main() // - 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 + vpImageIo::write(I, "Klimt.pgm"); // Write the image in a PGM P5 image file format } \endcode */ -void vpImageTools::changeLUT(vpImage &I, unsigned char A, - unsigned char A_star, unsigned char B, +void vpImageTools::changeLUT(vpImage &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")); + throw(vpImageException(vpImageException::incorrectInitializationError, "Bad gray levels")); } unsigned char v; @@ -141,25 +137,20 @@ void vpImageTools::changeLUT(vpImage &I, unsigned char A, \param I2 : The second image. \param Idiff : The result of the difference. */ -void vpImageTools::imageDifference(const vpImage &I1, - const vpImage &I2, +void vpImageTools::imageDifference(const vpImage &I1, const vpImage &I2, vpImage &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() != 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())) + if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) Idiff.resize(I1.getHeight(), I1.getWidth()); unsigned int n = I1.getHeight() * I1.getWidth(); for (unsigned int b = 0; b < n; b++) { int diff = I1.bitmap[b] - I2.bitmap[b] + 128; - Idiff.bitmap[b] = - (unsigned char)(vpMath::maximum(vpMath::minimum(diff, 255), 0)); + Idiff.bitmap[b] = (unsigned char)(vpMath::maximum(vpMath::minimum(diff, 255), 0)); } } @@ -176,21 +167,16 @@ void vpImageTools::imageDifference(const vpImage &I1, \param I2 : The second image. \param Idiff : The result of the difference between RGB components. */ -void vpImageTools::imageDifference(const vpImage &I1, - const vpImage &I2, - vpImage &Idiff) +void vpImageTools::imageDifference(const vpImage &I1, const vpImage &I2, vpImage &Idiff) { - if ((I1.getHeight() != I2.getHeight()) || - (I1.getWidth() != I2.getWidth())) { + if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) { throw(vpException(vpException::dimensionError, "Cannot compute image difference. The two images " "(%ux%u) and (%ux%u) have not the same size", - I1.getWidth(), I1.getHeight(), I2.getWidth(), - I2.getHeight())); + I1.getWidth(), I1.getHeight(), I2.getWidth(), I2.getHeight())); } - if ((I1.getHeight() != Idiff.getHeight()) || - (I1.getWidth() != Idiff.getWidth())) + if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) Idiff.resize(I1.getHeight(), I1.getWidth()); unsigned int n = I1.getHeight() * I1.getWidth(); @@ -199,14 +185,10 @@ void vpImageTools::imageDifference(const vpImage &I1, int diffG = I1.bitmap[b].G - I2.bitmap[b].G + 128; int diffB = I1.bitmap[b].B - I2.bitmap[b].B + 128; int diffA = I1.bitmap[b].A - I2.bitmap[b].A + 128; - Idiff.bitmap[b].R = - (unsigned char)(vpMath::maximum(vpMath::minimum(diffR, 255), 0)); - Idiff.bitmap[b].G = - (unsigned char)(vpMath::maximum(vpMath::minimum(diffG, 255), 0)); - Idiff.bitmap[b].B = - (unsigned char)(vpMath::maximum(vpMath::minimum(diffB, 255), 0)); - Idiff.bitmap[b].A = - (unsigned char)(vpMath::maximum(vpMath::minimum(diffA, 255), 0)); + Idiff.bitmap[b].R = (unsigned char)(vpMath::maximum(vpMath::minimum(diffR, 255), 0)); + Idiff.bitmap[b].G = (unsigned char)(vpMath::maximum(vpMath::minimum(diffG, 255), 0)); + Idiff.bitmap[b].B = (unsigned char)(vpMath::maximum(vpMath::minimum(diffB, 255), 0)); + Idiff.bitmap[b].A = (unsigned char)(vpMath::maximum(vpMath::minimum(diffA, 255), 0)); } } @@ -220,18 +202,14 @@ void vpImageTools::imageDifference(const vpImage &I1, \param I2 : The second image. \param Idiff : The result of the difference. */ -void vpImageTools::imageDifferenceAbsolute(const vpImage &I1, - const vpImage &I2, +void vpImageTools::imageDifferenceAbsolute(const vpImage &I1, const vpImage &I2, vpImage &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() != 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())) + if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) Idiff.resize(I1.getHeight(), I1.getWidth()); unsigned int n = I1.getHeight() * I1.getWidth(); @@ -254,18 +232,13 @@ void vpImageTools::imageDifferenceAbsolute(const vpImage &I1, \param I2 : The second image. \param Idiff : The result of the difference between RGB components. */ -void vpImageTools::imageDifferenceAbsolute(const vpImage &I1, - const vpImage &I2, - vpImage &Idiff) +void vpImageTools::imageDifferenceAbsolute(const vpImage &I1, const vpImage &I2, vpImage &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() != 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())) + if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth())) Idiff.resize(I1.getHeight(), I1.getWidth()); unsigned int n = I1.getHeight() * I1.getWidth(); @@ -291,18 +264,14 @@ void vpImageTools::imageDifferenceAbsolute(const vpImage &I1, \param saturate : If true, saturate the result to [0 ; 255] using vpMath::saturate, otherwise overflow may occur. */ -void vpImageTools::imageAdd(const vpImage &I1, - const vpImage &I2, +void vpImageTools::imageAdd(const vpImage &I1, const vpImage &I2, vpImage &Ires, const bool saturate) { - 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() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) { + throw(vpException(vpException::dimensionError, "The two images do not have the same size")); } - if ((I1.getHeight() != Ires.getHeight()) || - (I1.getWidth() != Ires.getWidth())) { + if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) { Ires.resize(I1.getHeight(), I1.getWidth()); } @@ -313,12 +282,10 @@ void vpImageTools::imageAdd(const vpImage &I1, #if VISP_HAVE_SSE2 if (vpCPUFeatures::checkSSE2() && Ires.getSize() >= 16) { - for (; cpt <= Ires.getSize() - 16; - cpt += 16, ptr_I1 += 16, ptr_I2 += 16, ptr_Ires += 16) { + for (; cpt <= Ires.getSize() - 16; cpt += 16, ptr_I1 += 16, ptr_I2 += 16, ptr_Ires += 16) { const __m128i v1 = _mm_loadu_si128((const __m128i *)ptr_I1); const __m128i v2 = _mm_loadu_si128((const __m128i *)ptr_I2); - const __m128i vres = - saturate ? _mm_adds_epu8(v1, v2) : _mm_add_epi8(v1, v2); + const __m128i vres = saturate ? _mm_adds_epu8(v1, v2) : _mm_add_epi8(v1, v2); _mm_storeu_si128((__m128i *)ptr_Ires, vres); } @@ -326,9 +293,7 @@ void vpImageTools::imageAdd(const vpImage &I1, #endif for (; cpt < Ires.getSize(); cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) { - *ptr_Ires = saturate ? vpMath::saturate( - (short int)*ptr_I1 + (short int)*ptr_I2) - : *ptr_I1 + *ptr_I2; + *ptr_Ires = saturate ? vpMath::saturate((short int)*ptr_I1 + (short int)*ptr_I2) : *ptr_I1 + *ptr_I2; } } @@ -341,19 +306,14 @@ void vpImageTools::imageAdd(const vpImage &I1, \param saturate : If true, saturate the result to [0 ; 255] using vpMath::saturate, otherwise overflow may occur. */ -void vpImageTools::imageSubtract(const vpImage &I1, - const vpImage &I2, - vpImage &Ires, - const bool saturate) +void vpImageTools::imageSubtract(const vpImage &I1, const vpImage &I2, + vpImage &Ires, const bool saturate) { - 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() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) { + throw(vpException(vpException::dimensionError, "The two images do not have the same size")); } - if ((I1.getHeight() != Ires.getHeight()) || - (I1.getWidth() != Ires.getWidth())) { + if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) { Ires.resize(I1.getHeight(), I1.getWidth()); } @@ -364,12 +324,10 @@ void vpImageTools::imageSubtract(const vpImage &I1, #if VISP_HAVE_SSE2 if (vpCPUFeatures::checkSSE2() && Ires.getSize() >= 16) { - for (; cpt <= Ires.getSize() - 16; - cpt += 16, ptr_I1 += 16, ptr_I2 += 16, ptr_Ires += 16) { + for (; cpt <= Ires.getSize() - 16; cpt += 16, ptr_I1 += 16, ptr_I2 += 16, ptr_Ires += 16) { const __m128i v1 = _mm_loadu_si128((const __m128i *)ptr_I1); const __m128i v2 = _mm_loadu_si128((const __m128i *)ptr_I2); - const __m128i vres = - saturate ? _mm_subs_epu8(v1, v2) : _mm_sub_epi8(v1, v2); + const __m128i vres = saturate ? _mm_subs_epu8(v1, v2) : _mm_sub_epi8(v1, v2); _mm_storeu_si128((__m128i *)ptr_Ires, vres); } @@ -377,9 +335,7 @@ void vpImageTools::imageSubtract(const vpImage &I1, #endif for (; cpt < Ires.getSize(); cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) { - *ptr_Ires = saturate ? vpMath::saturate( - (short int)*ptr_I1 - (short int)*ptr_I2) - : *ptr_I1 - *ptr_I2; + *ptr_Ires = saturate ? vpMath::saturate((short int)*ptr_I1 - (short int)*ptr_I2) : *ptr_I1 - *ptr_I2; } } @@ -390,8 +346,7 @@ void vpImageTools::imageSubtract(const vpImage &I1, // When t is 1, this will return C. In between values will return an // interpolation between B and C. A and B are used to calculate the slopes at // the edges. -float vpImageTools::cubicHermite(const float A, const float B, const float C, - const float D, const float t) +float vpImageTools::cubicHermite(const float A, const float B, const float C, const float D, const float t) { float a = (-A + 3.0f * B - 3.0f * C + D) / 2.0f; float b = A + 2.0f * C - (5.0f * B + D) / 2.0f; @@ -401,7 +356,4 @@ float vpImageTools::cubicHermite(const float A, const float B, const float C, return a * t * t * t + b * t * t + c * t + d; } -float vpImageTools::lerp(const float A, const float B, const float t) -{ - return A * (1.0f - t) + B * t; -} +float vpImageTools::lerp(const float A, const float B, const float t) { return A * (1.0f - t) + B * t; } diff --git a/modules/core/src/image/vpRGBa.cpp b/modules/core/src/image/vpRGBa.cpp index 3abe250f71..bbebf77cd3 100644 --- a/modules/core/src/image/vpRGBa.cpp +++ b/modules/core/src/image/vpRGBa.cpp @@ -133,10 +133,7 @@ bool vpRGBa::operator==(const vpRGBa &v) \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); -} +bool vpRGBa::operator!=(const vpRGBa &v) { return (R != v.R || G != v.G || B != v.B || A != v.A); } /*! Substraction operator : "this" - v. @@ -255,8 +252,9 @@ vpRGBa operator*(const double &x, const vpRGBa &rgb) { return rgb * x; } returns a reference to the stream. The coordinates are separated by a comma. - The following code prints the intensity of the pixel in the middle of the -image: \code #include + The following code prints the intensity of the pixel in the middle of the image: +\code +#include int main() { @@ -270,7 +268,6 @@ int main() */ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpRGBa &rgba) { - os << "(" << (int)rgba.R << "," << (int)rgba.G << "," << (int)rgba.B << "," - << (int)rgba.A << ")"; + os << "(" << (int)rgba.R << "," << (int)rgba.G << "," << (int)rgba.B << "," << (int)rgba.A << ")"; return os; } diff --git a/modules/core/src/math/kalman/vpKalmanFilter.cpp b/modules/core/src/math/kalman/vpKalmanFilter.cpp index 19896fd69d..b3852663e0 100644 --- a/modules/core/src/math/kalman/vpKalmanFilter.cpp +++ b/modules/core/src/math/kalman/vpKalmanFilter.cpp @@ -58,9 +58,7 @@ \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) +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; @@ -92,8 +90,8 @@ void vpKalmanFilter::init(unsigned int size_state_vector, */ 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() + : 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() { } @@ -105,9 +103,8 @@ vpKalmanFilter::vpKalmanFilter() \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() + : 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() { } @@ -124,11 +121,9 @@ vpKalmanFilter::vpKalmanFilter(unsigned int n_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() +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); } @@ -152,8 +147,7 @@ vpKalmanFilter::vpKalmanFilter(unsigned int size_state_vector, void vpKalmanFilter::prediction() { if (Xest.getRows() != size_state * nsignal) { - std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows() << " " - << size_state * nsignal << std::endl; + std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows() << " " << size_state * nsignal << std::endl; std::cout << " Error : Filter non initialized " << std::endl; exit(1); } diff --git a/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp b/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp index 49f04687eb..51caa275f9 100644 --- a/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp +++ b/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp @@ -125,12 +125,11 @@ int main() // Set the constant velocity state model used for the filtering vpLinearKalmanFilterInstantiation::vpStateModel model; - model = -vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel; + 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(); + // 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; @@ -139,9 +138,8 @@ vector int state_size = kalman.getStateSize(); // - 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) + 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 @@ -173,28 +171,22 @@ vector int state_size = kalman.getStateSize(); } \endcode */ -void vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal, - vpColVector &sigma_state, - vpColVector &sigma_measure, - double rho, double delta_t) +void vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal, vpColVector &sigma_state, + vpColVector &sigma_measure, double rho, double delta_t) { switch (model) { case stateConstVelWithColoredNoise_MeasureVel: - initStateConstVelWithColoredNoise_MeasureVel(n_signal, sigma_state, - sigma_measure, rho); + initStateConstVelWithColoredNoise_MeasureVel(n_signal, sigma_state, sigma_measure, rho); break; case stateConstVel_MeasurePos: - initStateConstVel_MeasurePos(n_signal, sigma_state, sigma_measure, - delta_t); + 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); + 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")); + throw(vpException(vpException::notInitialized, "Kalman state model is not set")); } } @@ -293,9 +285,8 @@ void vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal, \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) +void vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(unsigned int n_signal, vpColVector &sigma_state, + vpColVector &sigma_measure, double delta_t) { // init_done = true ; setStateModel(stateConstVel_MeasurePos); @@ -470,9 +461,8 @@ int main() // 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) + 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 @@ -481,8 +471,7 @@ int main() double rho = 0.9; // Initialize the filter - kalman.initStateConstVelWithColoredNoise_MeasureVel(nsignal, sigma_state, -sigma_measure, rho); + kalman.initStateConstVelWithColoredNoise_MeasureVel(nsignal, sigma_state, sigma_measure, rho); // Does the filtering vpColVector vm(2); // Measured velocities @@ -502,11 +491,10 @@ sigma_measure, rho); } \endcode */ -void vpLinearKalmanFilterInstantiation:: - initStateConstVelWithColoredNoise_MeasureVel(unsigned int n_signal, - vpColVector &sigma_state, - vpColVector &sigma_measure, - double rho) +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); @@ -688,11 +676,10 @@ int main() // 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) + 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 @@ -703,8 +690,7 @@ int main() double dt = 0.020; // 20 ms // Initialize the filter - kalman.initStateConstAccWithColoredNoise_MeasureVel(nsignal, sigma_state, -sigma_measure, rho, dt); + kalman.initStateConstAccWithColoredNoise_MeasureVel(nsignal, sigma_state, sigma_measure, rho, dt); // Does the filtering vpColVector vm(2); // Measured velocities @@ -725,11 +711,10 @@ sigma_measure, rho, dt); \endcode */ -void vpLinearKalmanFilterInstantiation:: - initStateConstAccWithColoredNoise_MeasureVel(unsigned int n_signal, - vpColVector &sigma_state, - vpColVector &sigma_measure, - double rho, double delta_t) +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); @@ -805,8 +790,7 @@ void vpLinearKalmanFilterInstantiation:: void vpLinearKalmanFilterInstantiation::filter(vpColVector &z) { if (nsignal < 1) { - vpERROR_TRACE( - "Bad signal number. You need to initialize the Kalman filter"); + vpERROR_TRACE("Bad signal number. You need to initialize the Kalman filter"); throw(vpException(vpException::notInitialized, "Bad signal number")); } @@ -825,8 +809,7 @@ void vpLinearKalmanFilterInstantiation::filter(vpColVector &z) break; case unknown: vpERROR_TRACE("Kalman state model is not set"); - throw(vpException(vpException::notInitialized, - "Kalman state model is not set")); + throw(vpException(vpException::notInitialized, "Kalman state model is not set")); break; } iter++; diff --git a/modules/core/src/math/matrix/vpColVector.cpp b/modules/core/src/math/matrix/vpColVector.cpp index 551e7fd5d5..b97d7261ba 100644 --- a/modules/core/src/math/matrix/vpColVector.cpp +++ b/modules/core/src/math/matrix/vpColVector.cpp @@ -58,8 +58,7 @@ #include #include -#if defined __SSE2__ || defined _M_X64 || \ - (defined _M_IX86_FP && _M_IX86_FP >= 2) +#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2) #include #define VISP_HAVE_SSE2 1 #endif @@ -68,10 +67,8 @@ 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())); + throw(vpException(vpException::dimensionError, "Cannot add (%dx1) column vector to (%dx1) column vector", getRows(), + v.getRows())); } vpColVector r(rowNum); @@ -85,8 +82,14 @@ vpColVector vpColVector::operator+(const vpColVector &v) const \param t : 3-dimension translation vector to add. \return The sum of the current columnn vector (*this) and the translation - vector to add. \code vpTranslationVector t1(1,2,3); vpColVector v(3); v[0] = - 4; v[1] = 5; v[2] = 6; vpTranslationVector t2; + vector to add. +\code + vpTranslationVector t1(1,2,3); + vpColVector v(3); + v[0] = 4; + v[1] = 5; + v[2] = 6; + vpTranslationVector t2; t2 = v + t1; // t1 and v leave unchanged @@ -97,10 +100,8 @@ vpColVector vpColVector::operator+(const vpColVector &v) const vpTranslationVector vpColVector::operator+(const vpTranslationVector &t) const { if (getRows() != 3) { - throw(vpException( - vpException::dimensionError, - "Cannot add %d-dimension column vector to a translation vector", - getRows())); + throw(vpException(vpException::dimensionError, "Cannot add %d-dimension column vector to a translation vector", + getRows())); } vpTranslationVector s; @@ -114,10 +115,8 @@ vpTranslationVector vpColVector::operator+(const vpTranslationVector &t) const 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())); + throw(vpException(vpException::dimensionError, "Cannot add (%dx1) column vector to (%dx1) column vector", getRows(), + v.getRows())); } for (unsigned int i = 0; i < rowNum; i++) @@ -128,10 +127,8 @@ vpColVector &vpColVector::operator+=(vpColVector v) 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())); + throw(vpException(vpException::dimensionError, "Cannot substract (%dx1) column vector to (%dx1) column vector", + getRows(), v.getRows())); } for (unsigned int i = 0; i < rowNum; i++) @@ -210,9 +207,7 @@ vpColVector vpColVector::operator-(const vpColVector &m) const \sa init() */ -vpColVector::vpColVector(const vpColVector &v, unsigned int r, - unsigned int nrows) - : vpArray2D(nrows, 1) +vpColVector::vpColVector(const vpColVector &v, unsigned int r, unsigned int nrows) : vpArray2D(nrows, 1) { init(v, r, nrows); } @@ -222,8 +217,9 @@ vpColVector::vpColVector(const vpColVector &v, unsigned int r, \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. + 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. @@ -253,16 +249,13 @@ v: 0 1 2 3 w: 1 2 \endcode */ -void vpColVector::init(const vpColVector &v, unsigned int r, - unsigned int nrows) +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())); + throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpColVector", rnrows, + v.getRows())); resize(nrows, false); if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced @@ -271,30 +264,26 @@ void vpColVector::init(const vpColVector &v, unsigned int r, (*this)[i - r] = v[i]; } -vpColVector::vpColVector(const vpRotationVector &v) - : vpArray2D(v.size(), 1) +vpColVector::vpColVector(const vpRotationVector &v) : vpArray2D(v.size(), 1) { for (unsigned int i = 0; i < v.size(); i++) (*this)[i] = v[i]; } -vpColVector::vpColVector(const vpPoseVector &p) - : vpArray2D(p.size(), 1) +vpColVector::vpColVector(const vpPoseVector &p) : vpArray2D(p.size(), 1) { for (unsigned int i = 0; i < p.size(); i++) (*this)[i] = p[i]; } -vpColVector::vpColVector(const vpTranslationVector &v) - : vpArray2D(v.size(), 1) +vpColVector::vpColVector(const vpTranslationVector &v) : vpArray2D(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(M.getRows(), 1) +vpColVector::vpColVector(const vpMatrix &M, unsigned int j) : vpArray2D(M.getRows(), 1) { for (unsigned int i = 0; i < M.getCols(); i++) (*this)[i] = M[i][j]; @@ -306,14 +295,11 @@ vpColVector::vpColVector(const vpMatrix &M, unsigned int j) \exception vpException::dimensionError If the matrix is not a m-by-1 matrix. */ -vpColVector::vpColVector(const vpMatrix &M) - : vpArray2D(M.getRows(), 1) +vpColVector::vpColVector(const vpMatrix &M) : vpArray2D(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())); + 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++) @@ -323,8 +309,7 @@ vpColVector::vpColVector(const vpMatrix &M) /*! Constructor that creates a column vector from a std vector of double. */ -vpColVector::vpColVector(const std::vector &v) - : vpArray2D((unsigned int)v.size(), 1) +vpColVector::vpColVector(const std::vector &v) : vpArray2D((unsigned int)v.size(), 1) { for (unsigned int i = 0; i < v.size(); i++) (*this)[i] = v[i]; @@ -332,8 +317,7 @@ vpColVector::vpColVector(const std::vector &v) /*! Constructor that creates a column vector from a std vector of float. */ -vpColVector::vpColVector(const std::vector &v) - : vpArray2D((unsigned int)v.size(), 1) +vpColVector::vpColVector(const std::vector &v) : vpArray2D((unsigned int)v.size(), 1) { for (unsigned int i = 0; i < v.size(); i++) (*this)[i] = (double)(v[i]); @@ -500,10 +484,8 @@ vpColVector vpColVector::operator/(double x) const 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())); + throw(vpException(vpException::dimensionError, "Cannot transform a (%dx%d) matrix into a column vector", + M.getRows(), M.getCols())); } resize(M.getRows(), false); @@ -721,12 +703,10 @@ vpColVector operator*(const double &x, const vpColVector &v) 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")); + 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")); + throw(vpException(vpException::fatalError, "Cannot compute the dot product: second vector empty")); } if (a.size() != b.size()) { throw(vpException(vpException::dimensionError, @@ -787,8 +767,7 @@ vpColVector &vpColVector::normalize() vpColVector vpColVector::invSort(const vpColVector &v) { if (v.data == NULL) { - throw(vpException(vpException::fatalError, - "Cannot sort content of column vector: vector empty")); + throw(vpException(vpException::fatalError, "Cannot sort content of column vector: vector empty")); } vpColVector tab; tab = v; @@ -817,8 +796,7 @@ vpColVector vpColVector::invSort(const vpColVector &v) vpColVector vpColVector::sort(const vpColVector &v) { if (v.data == NULL) { - throw(vpException(vpException::fatalError, - "Cannot sort content of column vector: vector empty")); + throw(vpException(vpException::fatalError, "Cannot sort content of column vector: vector empty")); } vpColVector tab; tab = v; @@ -881,10 +859,7 @@ void vpColVector::stack(const double &d) \sa stack(const vpColVector &, const vpColVector &, vpColVector &) */ -void vpColVector::stack(const vpColVector &v) -{ - *this = vpColVector::stack(*this, v); -} +void vpColVector::stack(const vpColVector &v) { *this = vpColVector::stack(*this, v); } /*! Stack column vectors. @@ -929,8 +904,7 @@ vpColVector vpColVector::stack(const vpColVector &A, const vpColVector &B) \sa stack(const vpColVector &) \sa stack(const vpColVector &, const vpColVector &) */ -void vpColVector::stack(const vpColVector &A, const vpColVector &B, - vpColVector &C) +void vpColVector::stack(const vpColVector &A, const vpColVector &B, vpColVector &C) { unsigned int nrA = A.getRows(); unsigned int nrB = B.getRows(); @@ -966,8 +940,7 @@ void vpColVector::stack(const vpColVector &A, const vpColVector &B, double vpColVector::mean(const vpColVector &v) { if (v.data == NULL || v.size() == 0) { - throw(vpException(vpException::dimensionError, - "Cannot compute column vector mean: vector empty")); + throw(vpException(vpException::dimensionError, "Cannot compute column vector mean: vector empty")); } // Use directly sum() function @@ -987,8 +960,7 @@ double vpColVector::mean(const vpColVector &v) double vpColVector::median(const vpColVector &v) { if (v.data == NULL || v.size() == 0) { - throw(vpException(vpException::dimensionError, - "Cannot compute column vector median: vector empty")); + throw(vpException(vpException::dimensionError, "Cannot compute column vector median: vector empty")); } std::vector vectorOfDoubles(v.data, v.data + v.rowNum); @@ -999,12 +971,10 @@ double vpColVector::median(const vpColVector &v) /*! Compute the standard deviation value of all the elements of the vector. */ -double vpColVector::stdev(const vpColVector &v, - const bool useBesselCorrection) +double vpColVector::stdev(const vpColVector &v, const bool useBesselCorrection) { if (v.data == NULL || v.size() == 0) { - throw(vpException(vpException::dimensionError, - "Cannot compute column vector stdev: vector empty")); + throw(vpException(vpException::dimensionError, "Cannot compute column vector stdev: vector empty")); } double mean_value = mean(v); @@ -1070,10 +1040,8 @@ 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())); + throw(vpException(vpException::dimensionError, "Cannot compute skew vector of a non 3-dimention vector (%d)", + v.getRows())); } M.resize(3, 3, false, false); @@ -1120,8 +1088,7 @@ vpColVector vpColVector::crossProd(const vpColVector &a, const vpColVector &b) \sa reshape(vpMatrix &, const unsigned int &, const unsigned int &) */ -vpMatrix vpColVector::reshape(const unsigned int &nrows, - const unsigned int &ncols) +vpMatrix vpColVector::reshape(const unsigned int &nrows, const unsigned int &ncols) { vpMatrix M(nrows, ncols); reshape(M, nrows, ncols); @@ -1183,13 +1150,11 @@ column vector: 9 10 11 12 \endcode */ -void vpColVector::reshape(vpMatrix &M, const unsigned int &nrows, - const unsigned int &ncols) +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())); + throw(vpException(vpException::dimensionError, "Cannot reshape (%dx1) column vector in (%dx%d) matrix", rowNum, + M.getRows(), M.getCols())); } if ((M.getRows() != nrows) || (M.getCols() != ncols)) M.resize(nrows, ncols, false, false); @@ -1234,8 +1199,7 @@ v: 0 10 11 3 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")); + throw(vpException(vpException::dimensionError, "Unable to insert a column vector")); if (data != NULL && v.data != NULL && v.rowNum > 0) { memcpy(data + i, v.data, sizeof(double) * v.rowNum); @@ -1261,8 +1225,7 @@ void vpColVector::insert(unsigned int i, const vpColVector &v) \sa std::ostream &operator<<(std::ostream &s, const vpArray2D &A) */ -int vpColVector::print(std::ostream &s, unsigned int length, - char const *intro) const +int vpColVector::print(std::ostream &s, unsigned int length, char const *intro) const { typedef std::string::size_type size_type; @@ -1407,8 +1370,7 @@ double vpColVector::sumSquare() const if (rowNum >= 4) { for (; i <= rowNum - 4; i += 4) { v_mul1 = _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(data + i)); - v_mul2 = _mm_mul_pd(_mm_loadu_pd(data + i + 2), - _mm_loadu_pd(data + i + 2)); + v_mul2 = _mm_mul_pd(_mm_loadu_pd(data + i + 2), _mm_loadu_pd(data + i + 2)); v_sum = _mm_add_pd(v_mul1, v_sum); v_sum = _mm_add_pd(v_mul2, v_sum); @@ -1463,8 +1425,7 @@ double vpColVector::euclideanNorm() const vpColVector vpColVector::hadamard(const vpColVector &v) const { if (v.getRows() != rowNum || v.getCols() != colNum) { - throw(vpException(vpException::dimensionError, - "Hadamard product: bad dimensions!")); + throw(vpException(vpException::dimensionError, "Hadamard product: bad dimensions!")); } vpColVector out; @@ -1475,8 +1436,7 @@ vpColVector vpColVector::hadamard(const vpColVector &v) const #if VISP_HAVE_SSE2 if (vpCPUFeatures::checkSSE2() && dsize >= 2) { for (; i <= dsize - 2; i += 2) { - __m128d vout = - _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(v.data + i)); + __m128d vout = _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(v.data + i)); _mm_storeu_pd(out.data + i, vout); } } @@ -1538,27 +1498,20 @@ vpColVector v (3); v[0] = 0; v[1] = 1; v[2] = 2; - \endcode */ -std::ostream &vpColVector::cppPrint(std::ostream &os, - const std::string &matrixName, - bool octet) const +std::ostream &vpColVector::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const { - os << "vpColVector " << matrixName << " (" << this->getRows() << "); " - << std::endl; + os << "vpColVector " << matrixName << " (" << this->getRows() << "); " << std::endl; for (unsigned int i = 0; i < this->getRows(); ++i) { if (!octet) { - os << matrixName << "[" << i << "] = " << (*this)[i] << "; " - << std::endl; + os << matrixName << "[" << i << "] = " << (*this)[i] << "; " << std::endl; } else { for (unsigned int k = 0; k < sizeof(double); ++k) { - os << "((unsigned char*)&(" << matrixName << "[" << i << "]) )[" << k - << "] = 0x" << std::hex - << (unsigned int)((unsigned char *)&((*this)[i]))[k] << "; " - << std::endl; + os << "((unsigned char*)&(" << matrixName << "[" << i << "]) )[" << k << "] = 0x" << std::hex + << (unsigned int)((unsigned char *)&((*this)[i]))[k] << "; " << std::endl; } } } @@ -1704,8 +1657,7 @@ std::ostream &vpColVector::matlabPrint(std::ostream &os) const \param c : Not used. */ -void vpColVector::insert(const vpColVector &v, const unsigned int r, - const unsigned int c) +void vpColVector::insert(const vpColVector &v, const unsigned int r, const unsigned int c) { (void)c; insert(r, v); diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp index 27060e18c2..a48ea092dc 100644 --- a/modules/core/src/math/matrix/vpMatrix.cpp +++ b/modules/core/src/math/matrix/vpMatrix.cpp @@ -67,8 +67,7 @@ #include #define USE_SSE_CODE 1 -#if defined __SSE2__ || defined _M_X64 || \ - (defined _M_IX86_FP && _M_IX86_FP >= 2) +#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2) #include #define VISP_HAVE_SSE2 1 #endif @@ -80,10 +79,8 @@ // Prototypes of specific functions vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int); -void compute_pseudo_inverse(const vpMatrix &a, const vpColVector &sv, - const vpMatrix &v, unsigned int nrows, - unsigned int ncols, unsigned int nrows_orig, - unsigned int ncols_orig, double svThreshold, +void compute_pseudo_inverse(const vpMatrix &a, const vpColVector &sv, const vpMatrix &v, unsigned int nrows, + unsigned int ncols, unsigned int nrows_orig, unsigned int ncols_orig, double svThreshold, vpMatrix &Ap, unsigned int &rank) { vpMatrix a1(ncols, nrows); @@ -118,11 +115,9 @@ void compute_pseudo_inverse(const vpMatrix &a, const vpColVector &sv, Ap = a1.t(); } -void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, - const vpMatrix &V, unsigned int nrows_orig, - unsigned int ncols_orig, double svThreshold, - vpMatrix &Ap, unsigned int &rank, vpMatrix &imA, - vpMatrix &imAt, vpMatrix &kerAt) +void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows_orig, + unsigned int ncols_orig, double svThreshold, vpMatrix &Ap, unsigned int &rank, + vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt) { Ap.resize(ncols_orig, nrows_orig); @@ -168,8 +163,7 @@ void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, for (unsigned int j = 0, k = 0; j < ncols_orig; 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::epsilon())) { + (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits::epsilon())) { for (unsigned int i = 0; i < V.getRows(); i++) { kerAt[k][i] = V[i][j]; } @@ -184,8 +178,7 @@ void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, \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) +vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) : vpArray2D() { if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) { @@ -262,22 +255,17 @@ N [2,3]= \sa extract() */ -void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, - unsigned int nrows, unsigned int ncols) +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())); + 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())); + throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols, + M.getCols())); resize(nrows, ncols, false, false); if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced @@ -328,22 +316,17 @@ N [2,3]= \sa init(const vpMatrix &, unsigned int, unsigned int, unsigned int, unsigned int) */ -vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, - unsigned int ncols) const +vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const { unsigned int rnrows = r + nrows; unsigned int cncols = c + ncols; if (rnrows > getRows()) - throw( - vpException(vpException::dimensionError, - "Bad row dimension (%d > %d) used to initialize vpMatrix", - rnrows, getRows())); + throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows, + getRows())); if (cncols > getCols()) - throw(vpException( - vpException::dimensionError, - "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols, - getCols())); + throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols, + getCols())); vpMatrix M(nrows, ncols); for (unsigned int i = 0; i < nrows; i++) { @@ -505,8 +488,7 @@ void vpMatrix::AtA(vpMatrix &B) const char transa = 'n'; char transb = 't'; - vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, - colNum, data, colNum, beta, B.data, colNum); + vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data, colNum); #else unsigned int i, j, k; double s; @@ -557,7 +539,11 @@ vpMatrix vpMatrix::AtA() const \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 + matrix: +\code +vpRotationMatrix R; +vpMatrix M = R; +\endcode */ vpMatrix &vpMatrix::operator=(const vpArray2D &A) @@ -740,10 +726,8 @@ 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())); + 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++) @@ -777,14 +761,11 @@ vpColVector vpMatrix::operator*(const vpColVector &v) const \sa operator*(const vpColVector &v) const */ -void vpMatrix::multMatrixVector(const vpMatrix &A, const vpColVector &v, - vpColVector &w) +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())); + throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector", + A.getRows(), A.getCols(), v.getRows())); } if (A.rowNum != w.rowNum) @@ -796,8 +777,7 @@ void vpMatrix::multMatrixVector(const vpMatrix &A, const vpColVector &v, char trans = 't'; int incr = 1; - vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, - v.data, incr, beta, w.data, incr); + vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr); #else w = 0.0; for (unsigned int j = 0; j < A.colNum; j++) { @@ -822,16 +802,14 @@ void vpMatrix::multMatrixVector(const vpMatrix &A, const vpColVector &v, \sa operator*() */ -void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, - vpMatrix &C) +void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) { if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum, B.colNum, false, false); 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())); + throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(), + A.getCols(), B.getRows(), B.getCols())); } #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) @@ -839,9 +817,8 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, double beta = 0.0; char trans = 'n'; - vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, - B.data, B.colNum, A.data, A.colNum, beta, C.data, - B.colNum); + vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta, + C.data, B.colNum); #else // 5/12/06 some "very" simple optimization to avoid indexation unsigned int BcolNum = B.colNum; @@ -874,8 +851,7 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, \exception vpException::dimensionError If matrices are not 3-by-3 dimension. */ -void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, - vpRotationMatrix &C) +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, @@ -914,8 +890,7 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, \exception vpException::dimensionError If matrices are not 4-by-4 dimension. */ -void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, - vpHomogeneousMatrix &C) +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, @@ -953,8 +928,7 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, \sa multMatrixVector() */ -void vpMatrix::mult2Matrices(const vpMatrix &A, const vpColVector &B, - vpColVector &C) +void vpMatrix::mult2Matrices(const vpMatrix &A, const vpColVector &B, vpColVector &C) { vpMatrix::multMatrixVector(A, B, C); } @@ -979,10 +953,8 @@ vpMatrix vpMatrix::operator*(const vpMatrix &B) const 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)); + throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum, + colNum)); } vpMatrix C(rowNum, 3); @@ -1008,10 +980,8 @@ vpMatrix vpMatrix::operator*(const vpRotationMatrix &R) const vpMatrix vpMatrix::operator*(const vpVelocityTwistMatrix &V) const { if (colNum != V.getRows()) { - throw(vpException( - vpException::dimensionError, - "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix", - rowNum, colNum)); + throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix", + rowNum, colNum)); } vpMatrix M; M.resize(rowNum, 6, false, false); @@ -1021,8 +991,8 @@ vpMatrix vpMatrix::operator*(const vpVelocityTwistMatrix &V) const double beta = 0.0; char trans = 'n'; - vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, - V.colNum, data, colNum, beta, M.data, V.colNum); + vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data, + V.colNum); #else bool checkSSE2 = vpCPUFeatures::checkSSE2(); #if !USE_SSE @@ -1045,8 +1015,7 @@ vpMatrix vpMatrix::operator*(const vpVelocityTwistMatrix &V) const for (int j = 0; j < 6; j++) { __m128d v_mul = _mm_setzero_pd(); for (int k = 0; k < 6; k += 2) { - v_mul = _mm_add_pd(v_mul, _mm_mul_pd(_mm_loadu_pd(&rowptri[k]), - _mm_loadu_pd(&V_trans[j][k]))); + v_mul = _mm_add_pd(v_mul, _mm_mul_pd(_mm_loadu_pd(&rowptri[k]), _mm_loadu_pd(&V_trans[j][k]))); } double v_tmp[2]; @@ -1080,10 +1049,8 @@ vpMatrix vpMatrix::operator*(const vpVelocityTwistMatrix &V) const vpMatrix vpMatrix::operator*(const vpForceTwistMatrix &V) const { if (colNum != V.getRows()) { - throw(vpException( - vpException::dimensionError, - "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix", - rowNum, colNum)); + throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix", + rowNum, colNum)); } vpMatrix M(rowNum, 6); @@ -1113,17 +1080,15 @@ A new matrix won't be allocated for every use of the function \sa operator+() */ -void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, - const vpMatrix &B, const double &wB, +void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C) { if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum, B.colNum, false, false); 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())); + 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; @@ -1150,9 +1115,8 @@ void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) C.resize(A.rowNum, B.colNum, false, false); 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())); + 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; @@ -1178,16 +1142,14 @@ void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) \sa vpColVector::operator+() */ -void vpMatrix::add2Matrices(const vpColVector &A, const vpColVector &B, - vpColVector &C) +void vpMatrix::add2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C) { if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum); 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())); + 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; @@ -1227,16 +1189,14 @@ vpMatrix vpMatrix::operator+(const vpMatrix &B) const \sa vpColVector::operator-() */ -void vpMatrix::sub2Matrices(const vpColVector &A, const vpColVector &B, - vpColVector &C) +void vpMatrix::sub2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C) { if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) C.resize(A.rowNum); 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())); + 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; @@ -1268,9 +1228,8 @@ void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) C.resize(A.rowNum, A.colNum, false, false); 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())); + 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; @@ -1300,9 +1259,8 @@ vpMatrix vpMatrix::operator-(const vpMatrix &B) const 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())); + throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum, + B.getRows(), B.getCols())); } double **BrowPtrs = B.rowPtrs; @@ -1318,9 +1276,8 @@ vpMatrix &vpMatrix::operator+=(const vpMatrix &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())); + throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum, + B.getRows(), B.getCols())); } double **BrowPtrs = B.rowPtrs; @@ -1427,8 +1384,7 @@ vpMatrix vpMatrix::operator/(double x) const // if (x == 0) { if (std::fabs(x) <= std::numeric_limits::epsilon()) { - throw vpException(vpException::divideByZeroError, - "Divide matrix by zero scalar"); + throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar"); } double xinv = 1 / x; @@ -1478,8 +1434,7 @@ vpMatrix &vpMatrix::operator/=(double x) { // if (x == 0) if (std::fabs(x) <= std::numeric_limits::epsilon()) - throw vpException(vpException::divideByZeroError, - "Divide matrix by zero scalar"); + throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar"); double xinv = 1 / x; @@ -1557,8 +1512,7 @@ vpRowVector vpMatrix::stackRows() vpMatrix vpMatrix::hadamard(const vpMatrix &m) const { if (m.getRows() != rowNum || m.getCols() != colNum) { - throw(vpException(vpException::dimensionError, - "In Hadamard product: bad dimension of input matrix")); + throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix")); } vpMatrix out; @@ -1569,8 +1523,7 @@ vpMatrix vpMatrix::hadamard(const vpMatrix &m) const #if VISP_HAVE_SSE2 if (vpCPUFeatures::checkSSE2() && dsize >= 2) { for (; i <= dsize - 2; i += 2) { - __m128d vout = - _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(m.data + i)); + __m128d vout = _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(m.data + i)); _mm_storeu_pd(out.data + i, vout); } } @@ -1598,8 +1551,7 @@ void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out) 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")); + throw(vpException(vpException::dimensionError, "In Kronecker product bad dimension of output matrix")); } for (unsigned int r = 0; r < r1; r++) { @@ -1623,10 +1575,7 @@ void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out) \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); -} +void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); } /*! Compute Kronecker product matrix. @@ -1716,10 +1665,7 @@ std::cout << "X:\n" << X << std::endl; \sa solveBySVD(const vpColVector &) */ -void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const -{ - x = pseudoInverse(1e-6) * b; -} +void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; } /*! @@ -1855,9 +1801,7 @@ void vpMatrix::svd(vpColVector &w, vpMatrix &V) #else (void)w; (void)V; - throw(vpException( - vpException::fatalError, - "Cannot compute SVD. Install Lapack, Eigen3, OpenCV or GSL 3rd party")); + throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3, OpenCV or GSL 3rd party")); #endif } @@ -2069,8 +2013,7 @@ vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const U.svdLapack(sv, V); unsigned int rank; - compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, - svThreshold, Ap, rank); + compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank); return Ap; } @@ -2115,8 +2058,7 @@ int main() \sa pseudoInverse(vpMatrix &, double) const */ -unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, - double svThreshold) const +unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const { unsigned int nrows, ncols; unsigned int nrows_orig = getRows(); @@ -2144,8 +2086,7 @@ unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, U.svdLapack(sv, V); - compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, - svThreshold, Ap, rank); + compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank); return rank; } @@ -2196,8 +2137,7 @@ int main() \sa pseudoInverse(vpMatrix &, vpColVector &, double) const */ -unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, - double svThreshold) const +unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const { unsigned int nrows, ncols; unsigned int nrows_orig = getRows(); @@ -2225,8 +2165,7 @@ unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, U.svdLapack(sv, V); - compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, - svThreshold, Ap, rank); + compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank); return rank; } @@ -2338,10 +2277,8 @@ int main() \sa pseudoInverse(vpMatrix &, vpColVector &, double, vpMatrix &, vpMatrix &, vpMatrix &) const */ -unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, - double svThreshold, vpMatrix &imA, - vpMatrix &imAt, - vpMatrix &kerA) const +unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, + vpMatrix &imAt, vpMatrix &kerA) const { unsigned int nrows = getRows(); unsigned int ncols = getCols(); @@ -2360,8 +2297,7 @@ unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, U.insert(*this, 0, 0); U.svdLapack(sv_, V); - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, - imAt, kerA); + compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA); // Remove singular values equal to to that correspond to the lines of 0 // introduced when m < n @@ -2436,8 +2372,7 @@ vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const U.svdEigen3(sv, V); unsigned int rank; - compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, - svThreshold, Ap, rank); + compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank); return Ap; } @@ -2482,8 +2417,7 @@ int main() \sa pseudoInverse(vpMatrix &, double) const */ -unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, - double svThreshold) const +unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const { unsigned int nrows, ncols; unsigned int nrows_orig = getRows(); @@ -2511,8 +2445,7 @@ unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, U.svdEigen3(sv, V); - compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, - svThreshold, Ap, rank); + compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank); return rank; } @@ -2563,8 +2496,7 @@ int main() \sa pseudoInverse(vpMatrix &, vpColVector &, double) const */ -unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, - double svThreshold) const +unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const { unsigned int nrows, ncols; unsigned int nrows_orig = getRows(); @@ -2592,8 +2524,7 @@ unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, U.svdEigen3(sv, V); - compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, - svThreshold, Ap, rank); + compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank); return rank; } @@ -2705,10 +2636,8 @@ int main() \sa pseudoInverse(vpMatrix &, vpColVector &, double, vpMatrix &, vpMatrix &, vpMatrix &) const */ -unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, - double svThreshold, vpMatrix &imA, - vpMatrix &imAt, - vpMatrix &kerA) const +unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, + vpMatrix &imAt, vpMatrix &kerA) const { unsigned int nrows = getRows(); unsigned int ncols = getCols(); @@ -2727,8 +2656,7 @@ unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, U.insert(*this, 0, 0); U.svdEigen3(sv_, V); - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, - imAt, kerA); + compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA); // Remove singular values equal to to that correspond to the lines of 0 // introduced when m < n @@ -2803,8 +2731,7 @@ vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const U.svdOpenCV(sv, V); unsigned int rank; - compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, - svThreshold, Ap, rank); + compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank); return Ap; } @@ -2849,8 +2776,7 @@ int main() \sa pseudoInverse(vpMatrix &, double) const */ -unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, - double svThreshold) const +unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const { unsigned int nrows, ncols; unsigned int nrows_orig = getRows(); @@ -2878,8 +2804,7 @@ unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, U.svdOpenCV(sv, V); - compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, - svThreshold, Ap, rank); + compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank); return rank; } @@ -2930,8 +2855,7 @@ int main() \sa pseudoInverse(vpMatrix &, vpColVector &, double) const */ -unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, - double svThreshold) const +unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const { unsigned int nrows, ncols; unsigned int nrows_orig = getRows(); @@ -2959,8 +2883,7 @@ unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, U.svdOpenCV(sv, V); - compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, - svThreshold, Ap, rank); + compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank); return rank; } @@ -3072,10 +2995,8 @@ int main() \sa pseudoInverse(vpMatrix &, vpColVector &, double, vpMatrix &, vpMatrix &, vpMatrix &) const */ -unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, - double svThreshold, vpMatrix &imA, - vpMatrix &imAt, - vpMatrix &kerA) const +unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, + vpMatrix &imAt, vpMatrix &kerA) const { unsigned int nrows = getRows(); unsigned int ncols = getCols(); @@ -3094,8 +3015,7 @@ unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, U.insert(*this, 0, 0); U.svdOpenCV(sv_, V); - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, - imAt, kerA); + compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA); // Remove singular values equal to to that correspond to the lines of 0 // introduced when m < n @@ -3170,8 +3090,7 @@ vpMatrix vpMatrix::pseudoInverseGsl(double svThreshold) const U.svdGsl(sv, V); unsigned int rank; - compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, - svThreshold, Ap, rank); + compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank); return Ap; } @@ -3216,8 +3135,7 @@ int main() \sa pseudoInverse(vpMatrix &, double) const */ -unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, - double svThreshold) const +unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, double svThreshold) const { unsigned int nrows, ncols; unsigned int nrows_orig = getRows(); @@ -3245,8 +3163,7 @@ unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, U.svdGsl(sv, V); - compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, - svThreshold, Ap, rank); + compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank); return rank; } @@ -3297,8 +3214,7 @@ int main() \sa pseudoInverse(vpMatrix &, vpColVector &, double) const */ -unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, - double svThreshold) const +unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold) const { unsigned int nrows, ncols; unsigned int nrows_orig = getRows(); @@ -3326,8 +3242,7 @@ unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, U.svdGsl(sv, V); - compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, - svThreshold, Ap, rank); + compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank); return rank; } @@ -3438,8 +3353,7 @@ int main() \sa pseudoInverse(vpMatrix &, vpColVector &, double, vpMatrix &, vpMatrix &, vpMatrix &) const */ -unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, - double svThreshold, vpMatrix &imA, +unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerA) const { unsigned int nrows = getRows(); @@ -3459,8 +3373,7 @@ unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, U.insert(*this, 0, 0); U.svdGsl(sv_, V); - compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, - imAt, kerA); + compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA); // Remove singular values equal to to that correspond to the lines of 0 // introduced when m < n @@ -3533,8 +3446,7 @@ Rank: 2 Singular values: 6.874359351 4.443330227 \endcode */ -unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, - double svThreshold) const +unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const { #if defined(VISP_HAVE_LAPACK) return pseudoInverseLapack(Ap, sv, svThreshold); @@ -3628,8 +3540,7 @@ Im(A^T): [3,2]= 0.845615 -0.102722 \endcode */ -unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, - double svThreshold, vpMatrix &imA, +unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt) const { vpMatrix kerAt; @@ -3770,9 +3681,8 @@ Im(A) * S * [Im(A^T) | Ker(A)]^T:[2,3]= -4 2 3 \endcode */ -unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, - double svThreshold, vpMatrix &imA, - vpMatrix &imAt, vpMatrix &kerAt) const +unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt, + vpMatrix &kerAt) const { #if defined(VISP_HAVE_LAPACK) return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt); @@ -3835,12 +3745,10 @@ column vector: 13 \endcode */ -vpColVector vpMatrix::getCol(const unsigned int j, const unsigned int i_begin, - const unsigned int column_size) const +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")); + 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]; @@ -3889,8 +3797,7 @@ column vector: vpColVector vpMatrix::getCol(const unsigned int j) const { if (j >= getCols()) - throw(vpException(vpException::dimensionError, - "Unable to extract a column vector from the matrix")); + 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++) @@ -3936,8 +3843,7 @@ Row vector: vpRowVector vpMatrix::getRow(const unsigned int i) const { if (i >= getRows()) - throw(vpException(vpException::dimensionError, - "Unable to extract a row vector from the matrix")); + throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix")); vpRowVector r; r.resize(colNum, false); @@ -3986,12 +3892,10 @@ Row vector: 5 6 7 \endcode */ -vpRowVector vpMatrix::getRow(const unsigned int i, const unsigned int j_begin, - const unsigned int row_size) const +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")); + 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]; @@ -4054,9 +3958,8 @@ void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) 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())); + throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(), + A.getCols(), B.getRows(), B.getCols())); } } @@ -4100,9 +4003,8 @@ void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C) 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())); + throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", A.getRows(), + A.getCols(), r.getCols())); } } @@ -4141,8 +4043,7 @@ void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C) \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 vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, const unsigned int r, const unsigned int c) { vpMatrix C; @@ -4164,17 +4065,14 @@ vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &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) +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())) { + if (((r + B.getRows()) <= A.getRows()) && ((c + B.getCols()) <= A.getCols())) { C.resize(A.getRows(), A.getCols(), false, false); 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())) { + 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]; @@ -4182,10 +4080,8 @@ void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, } } } 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); + 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); } } @@ -4221,17 +4117,15 @@ vpMatrix vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B) \warning A and B must have the same number of rows. */ -void vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B, - vpMatrix &C) +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())); + throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(), + A.getCols(), B.getRows(), B.getCols())); } } @@ -4269,8 +4163,7 @@ void vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B, \sa std::ostream &operator<<(std::ostream &s, const vpArray2D &A) */ -int vpMatrix::print(std::ostream &s, unsigned int length, - char const *intro) const +int vpMatrix::print(std::ostream &s, unsigned int length, char const *intro) const { typedef std::string::size_type size_type; @@ -4527,24 +4420,18 @@ M[1][2] = 5; \endcode */ -std::ostream &vpMatrix::cppPrint(std::ostream &os, - const std::string &matrixName, - bool octet) const +std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const { - os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " - << this->getCols() << "); " << std::endl; + os << "vpMatrix " << matrixName << " (" << 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 << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] - << "; " << std::endl; + os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl; } else { for (unsigned int k = 0; k < sizeof(double); ++k) { - os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j - << "]) )[" << k << "] = 0x" << std::hex - << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " - << std::endl; + os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex + << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl; } } } @@ -4563,9 +4450,8 @@ void vpMatrix::stack(const vpMatrix &A) *this = A; } else if (A.getRows() > 0) { if (colNum != A.getCols()) { - throw(vpException(vpException::dimensionError, - "Cannot stack (%dx%d) matrix with (%dx%d) matrix", - rowNum, colNum, A.getRows(), A.getCols())); + throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum, + A.getRows(), A.getCols())); } unsigned int rowNumOld = rowNum; @@ -4595,9 +4481,8 @@ void vpMatrix::stack(const vpRowVector &r) *this = r; } else { if (colNum != r.getCols()) { - throw(vpException(vpException::dimensionError, - "Cannot stack (%dx%d) matrix with (1x%d) row vector", - rowNum, colNum, r.getCols())); + throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum, + colNum, r.getCols())); } if (r.size() == 0) { @@ -4624,24 +4509,19 @@ void vpMatrix::stack(const vpRowVector &r) \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) +void vpMatrix::insert(const vpMatrix &A, const unsigned int r, const unsigned int c) { if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) { - if (A.colNum == colNum && data != NULL && A.data != NULL && - A.size() > 0) { + if (A.colNum == colNum && data != NULL && A.data != NULL && A.size() > 0) { memcpy(data + r * colNum, A.data, sizeof(double) * A.size()); } else if (data != NULL && A.data != NULL && A.colNum > 0) { for (unsigned int i = r; i < (r + A.getRows()); i++) { - memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, - sizeof(double) * A.colNum); + memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum); } } } 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); + 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); } } @@ -4686,10 +4566,8 @@ int main() vpColVector vpMatrix::eigenValues() const { if (rowNum != colNum) { - throw(vpException( - vpException::dimensionError, - "Cannot compute eigen values on a non square matrix (%dx%d)", 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 */ @@ -4700,9 +4578,7 @@ vpColVector vpMatrix::eigenValues() const for (unsigned int j = 0; j < rowNum; j++) { // if (At_A[i][j] != 0) { if (std::fabs(At_A[i][j]) > std::numeric_limits::epsilon()) { - throw(vpException( - vpException::fatalError, - "Cannot compute eigen values on a non symetric matrix")); + throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix")); } } } @@ -4738,9 +4614,8 @@ vpColVector vpMatrix::eigenValues() const } #else { - throw(vpException(vpException::functionNotImplementedError, - "Eigen values computation is not implemented. You " - "should install GSL rd party")); + throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. You " + "should install GSL rd party")); } #endif } @@ -4801,15 +4676,12 @@ int main() #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 +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)); + 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 */ @@ -4820,9 +4692,7 @@ void vpMatrix::eigenValues(vpColVector & /* evalue */, for (unsigned int j = 0; j < rowNum; j++) { // if (At_A[i][j] != 0) { if (std::fabs(At_A[i][j]) > std::numeric_limits::epsilon()) { - throw(vpException( - vpException::fatalError, - "Cannot compute eigen values on a non symetric matrix")); + throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix")); } } } @@ -4865,9 +4735,8 @@ void vpMatrix::eigenValues(vpColVector & /* evalue */, } #else { - throw(vpException(vpException::functionNotImplementedError, - "Eigen values computation is not implemented. You " - "should install GSL rd party")); + throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. You " + "should install GSL rd party")); } #endif } @@ -4933,8 +4802,7 @@ unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const for (unsigned int j = 0, k = 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::epsilon())) { + (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits::epsilon())) { for (unsigned int i = 0; i < V.getRows(); i++) { kerAt[k][i] = V[i][j]; } @@ -4969,12 +4837,11 @@ int main() std::cout << "Initial matrix: \n" << A << std::endl; // Compute the determinant - std:: cout << "Determinant by default method : " << A.det() << -std::endl; std:: cout << "Determinant by LU decomposition : " << -A.detByLU() << std::endl; std:: cout << "Determinant by LU decomposition -(Lapack): " << A.detByLULapack() << std::endl; std:: cout << "Determinant by -LU decomposition (OpenCV): " << A.detByLUOpenCV() << std::endl; std:: cout << -"Determinant by LU decomposition (GSL) : " << A.detByLUGsl() << std::endl; + std:: cout << "Determinant by default method : " << A.det() << std::endl; + std:: cout << "Determinant by LU decomposition : " << A.detByLU() << std::endl; + std:: cout << "Determinant by LU decomposition (Lapack): " << A.detByLULapack() << std::endl; + std:: cout << "Determinant by LU decomposition (OpenCV): " << A.detByLUOpenCV() << std::endl; + std:: cout << "Determinant by LU decomposition (GSL) : " << A.detByLUGsl() << std::endl; } \endcode */ @@ -4999,10 +4866,8 @@ double vpMatrix::det(vpDetMethod method) const vpMatrix vpMatrix::expm() const { if (colNum != rowNum) { - throw(vpException( - vpException::dimensionError, - "Cannot compute the exponential of a non square (%dx%d) matrix", - rowNum, colNum)); + 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; @@ -5145,13 +5010,11 @@ double vpMatrix::cond() const \param alpha : Scalar \f$\alpha\f$ \param HLM : Resulting operation. */ -void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, - vpMatrix &HLM) +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())); + throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(), + H.getCols())); } HLM.resize(H.getRows(), H.getCols(), false, false); @@ -5235,22 +5098,14 @@ 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) +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); -} +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); -} +void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); } /*! \deprecated This method is deprecated. You should use getRow(). diff --git a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp index b0c1242add..c412566290 100644 --- a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp +++ b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp @@ -60,10 +60,8 @@ typedef long int integer; typedef int integer; #endif -extern "C" void dpotrf_(char *uplo, integer *n, double *a, integer *lda, - integer *info); -extern "C" int dpotri_(char *uplo, integer *n, double *a, integer *lda, - integer *info); +extern "C" void dpotrf_(char *uplo, integer *n, double *a, integer *lda, integer *info); +extern "C" int dpotri_(char *uplo, integer *n, double *a, integer *lda, integer *info); #endif /*! @@ -159,24 +157,19 @@ int main() vpMatrix vpMatrix::inverseByCholeskyLapack() const { if (rowNum != colNum) { - throw(vpMatrixException( - vpMatrixException::matrixError, - "Cannot inverse a non-square matrix (%ux%u) by Cholesky", rowNum, - colNum)); + throw(vpMatrixException(vpMatrixException::matrixError, "Cannot inverse a non-square matrix (%ux%u) by Cholesky", + rowNum, colNum)); } integer rowNum_ = (integer)this->getRows(); - integer lda = (integer) - rowNum_; // lda is the number of rows because we don't use a submatrix + integer lda = (integer)rowNum_; // lda is the number of rows because we don't use a submatrix integer info; vpMatrix A = *this; dpotrf_((char *)"L", &rowNum_, A.data, &lda, &info); if (info != 0) - throw(vpException( - vpException::fatalError, - "Cannot inverse by Cholesky with Lapack: error in dpotrf_()")); + throw(vpException(vpException::fatalError, "Cannot inverse by Cholesky with Lapack: error in dpotrf_()")); dpotri_((char *)"L", &rowNum_, A.data, &lda, &info); if (info != 0) { @@ -234,9 +227,7 @@ vpMatrix vpMatrix::inverseByCholeskyOpenCV() const { if (rowNum != colNum) { throw( - vpException(vpException::fatalError, - "Cannot inverse a non square matrix (%ux%u) by Cholesky", - rowNum, colNum)); + vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by Cholesky", rowNum, colNum)); } cv::Mat M(rowNum, colNum, CV_64F, this->data); diff --git a/modules/core/src/math/matrix/vpMatrix_covariance.cpp b/modules/core/src/math/matrix/vpMatrix_covariance.cpp index f4c19cd451..f6983934db 100644 --- a/modules/core/src/math/matrix/vpMatrix_covariance.cpp +++ b/modules/core/src/math/matrix/vpMatrix_covariance.cpp @@ -56,19 +56,15 @@ \param b : Vector b from Ax = b. */ -vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, - const vpColVector &x, - const vpColVector &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 + double denom = ((double)(A.getRows())); // To consider MLE Estimate for sigma if (denom <= std::numeric_limits::epsilon()) - throw vpMatrixException( - vpMatrixException::divideByZeroError, - "Impossible to compute covariance matrix: not enough data"); + 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. @@ -76,9 +72,7 @@ vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, sigma2 /= denom; - return (A.t() * A).pseudoInverse(A.getCols() * - std::numeric_limits::epsilon()) * - sigma2; + return (A.t() * A).pseudoInverse(A.getCols() * std::numeric_limits::epsilon()) * sigma2; } /*! @@ -94,9 +88,7 @@ vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, \param W : Diagonal weigths matrix from WAx = Wb. */ -vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, - const vpColVector &x, - const vpColVector &b, +vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const vpMatrix &W) { double denom = 0.0; @@ -107,19 +99,15 @@ vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, } if (denom <= std::numeric_limits::epsilon()) - throw vpMatrixException( - vpMatrixException::divideByZeroError, - "Impossible to compute covariance matrix: not enough data"); + 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::epsilon()) * - sigma2; + return (A.t() * (W2)*A).pseudoInverse(A.getCols() * std::numeric_limits::epsilon()) * sigma2; } /*! @@ -133,8 +121,7 @@ vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, \param Ls : interaction matrix used in v = Ls.pseudoInverse() * DeltaS */ -vpMatrix vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, - const vpColVector &deltaS, +vpMatrix vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls) { vpMatrix Js; @@ -159,10 +146,8 @@ vpMatrix vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, \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 vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, + const vpMatrix &Ls, const vpMatrix &W) { vpMatrix Js; vpColVector deltaP; @@ -171,10 +156,8 @@ vpMatrix vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, return vpMatrix::computeCovarianceMatrix(Js, deltaP, deltaS, W); } -void vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, - const vpColVector &deltaS, - const vpMatrix &Ls, vpMatrix &Js, - vpColVector &deltaP) +void vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, + vpMatrix &Js, vpColVector &deltaP) { // building Lp vpMatrix LpInv(6, 6); @@ -221,8 +204,7 @@ void vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, // 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); + -(vpMath::sqr(vpMath::sinc(theta / 2.0)) * theta2u_skew - (1.0 - vpMath::sinc(theta)) * u_skew * u_skew); } // vpMatrix LthetauInv = Lthetau.inverseByLU(); @@ -241,7 +223,5 @@ void vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, Js = Ls * LpInv; // building deltaP - deltaP = (Js).pseudoInverse(Js.getRows() * - std::numeric_limits::epsilon()) * - deltaS; + deltaP = (Js).pseudoInverse(Js.getRows() * std::numeric_limits::epsilon()) * deltaS; } diff --git a/modules/core/src/math/matrix/vpMatrix_lu.cpp b/modules/core/src/math/matrix/vpMatrix_lu.cpp index 2ee60ec38e..93a1c4b0b6 100644 --- a/modules/core/src/math/matrix/vpMatrix_lu.cpp +++ b/modules/core/src/math/matrix/vpMatrix_lu.cpp @@ -58,10 +58,9 @@ typedef long int integer; typedef int integer; #endif -extern "C" int dgetrf_(integer *m, integer *n, double *a, integer *lda, - integer *ipiv, integer *info); -extern "C" void dgetri_(integer *n, double *a, integer *lda, integer *ipiv, - double *work, integer *lwork, integer *info); +extern "C" int dgetrf_(integer *m, integer *n, double *a, integer *lda, integer *ipiv, integer *info); +extern "C" void dgetri_(integer *n, double *a, integer *lda, integer *ipiv, double *work, integer *lwork, + integer *info); #endif #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1 @@ -139,9 +138,8 @@ vpMatrix vpMatrix::inverseByLU() const #elif defined(VISP_HAVE_GSL) return inverseByLUGsl(); #else - throw(vpException(vpException::fatalError, - "Cannot compute matrix determinant. Install Eigen3, " - "Lapack, OpenCV or GSL 3rd party")); + throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. Install Eigen3, " + "Lapack, OpenCV or GSL 3rd party")); #endif } @@ -173,9 +171,8 @@ int main() std::cout << "Initial matrix: \n" << A << std::endl; // Compute the determinant - std:: cout << "Determinant by default method : " << A.det() << -std::endl; std:: cout << "Determinant by LU decomposition : " << -A.detByLU() << std::endl; + std:: cout << "Determinant by default method : " << A.det() << std::endl; + std:: cout << "Determinant by LU decomposition : " << A.detByLU() << std::endl; } \endcode \sa detByLULapack(), detByLUEigen3(), detByLUOpenCV(), detByLUGsl() @@ -185,12 +182,9 @@ double vpMatrix::detByLU() const if (rowNum == 2 && colNum == 2) { return ((*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]); } else if (rowNum == 3 && colNum == 3) { - return ((*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - - (*this)[1][2] * (*this)[2][1]) - - (*this)[0][1] * ((*this)[1][0] * (*this)[2][2] - - (*this)[1][2] * (*this)[2][0]) + - (*this)[0][2] * ((*this)[1][0] * (*this)[2][1] - - (*this)[1][1] * (*this)[2][0])); + return ((*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) - + (*this)[0][1] * ((*this)[1][0] * (*this)[2][2] - (*this)[1][2] * (*this)[2][0]) + + (*this)[0][2] * ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0])); } else { #if defined(VISP_HAVE_LAPACK) return detByLULapack(); @@ -201,9 +195,8 @@ double vpMatrix::detByLU() const #elif defined(VISP_HAVE_GSL) return detByLUGsl(); #else - throw(vpException(vpException::fatalError, - "Cannot compute matrix determinant. Install Lapack, " - "Eigen3, OpenCV or GSL 3rd party")); + throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. Install Lapack, " + "Eigen3, OpenCV or GSL 3rd party")); #endif } } @@ -245,9 +238,7 @@ inverseByLUOpenCV() vpMatrix vpMatrix::inverseByLUGsl() const { if (rowNum != colNum) { - throw(vpException(vpException::fatalError, - "Cannot inverse a non square matrix (%ux%u) by LU", - rowNum, colNum)); + throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum)); } gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum); @@ -303,8 +294,7 @@ int main() std::cout << "Initial matrix: \n" << A << std::endl; // Compute the determinant - std:: cout << "Determinant by LU decomposition (GSL): " << A.detByLUGsl() << -std::endl; + std:: cout << "Determinant by LU decomposition (GSL): " << A.detByLUGsl() << std::endl; } \endcode \sa detByLU(), detByLUEigen3(), detByLUOpenCV(), detByLULapack() @@ -314,10 +304,8 @@ double vpMatrix::detByLUGsl() const double det = 0.; if (rowNum != colNum) { - throw(vpException( - vpException::fatalError, - "Cannot compute matrix determinant of a non square matrix (%ux%u)", - rowNum, colNum)); + throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)", + rowNum, colNum)); } gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum); @@ -379,9 +367,7 @@ inverseByLUOpenCV() vpMatrix vpMatrix::inverseByLULapack() const { if (rowNum != colNum) { - throw(vpException(vpException::fatalError, - "Cannot inverse a non square matrix (%ux%u) by LU", - rowNum, colNum)); + throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum)); } integer dim = (integer)rowNum; @@ -397,8 +383,7 @@ vpMatrix vpMatrix::inverseByLULapack() const if (info) { delete[] ipiv; delete[] work; - throw(vpException(vpException::fatalError, - "Lapack LU decomposition failed; info=%d", info)); + throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", info)); } dgetri_(&dim, A.data, &dim, &ipiv[1], work, &lwork, &info); @@ -429,8 +414,7 @@ int main() std::cout << "Initial matrix: \n" << A << std::endl; // Compute the determinant - std:: cout << "Determinant by LU decomposition (Lapack): " << -A.detByLULapack() << std::endl; + std:: cout << "Determinant by LU decomposition (Lapack): " << A.detByLULapack() << std::endl; } \endcode \sa detByLU(), detByLUEigen3(), detByLUGsl(), detByLUOpenCV() @@ -438,10 +422,8 @@ A.detByLULapack() << std::endl; double vpMatrix::detByLULapack() const { if (rowNum != colNum) { - throw(vpException( - vpException::fatalError, - "Cannot compute matrix determinant of a non square matrix (%ux%u)", - rowNum, colNum)); + throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)", + rowNum, colNum)); } integer dim = (integer)rowNum; @@ -454,8 +436,7 @@ double vpMatrix::detByLULapack() const dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info); if (info) { delete[] ipiv; - throw(vpException(vpException::fatalError, - "Lapack LU decomposition failed; info=%d", info)); + throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", info)); } double det = A[0][0]; @@ -512,9 +493,7 @@ inverseByLULapack() vpMatrix vpMatrix::inverseByLUOpenCV() const { if (rowNum != colNum) { - throw(vpException(vpException::fatalError, - "Cannot inverse a non square matrix (%ux%u) by LU", - rowNum, colNum)); + throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum)); } cv::Mat M(rowNum, colNum, CV_64F, this->data); @@ -547,8 +526,7 @@ int main() std::cout << "Initial matrix: \n" << A << std::endl; // Compute the determinant - std:: cout << "Determinant by LU decomposition (OpenCV): " << -A.detByLUOpenCV() << std::endl; + std:: cout << "Determinant by LU decomposition (OpenCV): " << A.detByLUOpenCV() << std::endl; } \endcode \sa detByLU(), detByLUEigen3(), detByLUGsl(), detByLULapack() @@ -558,10 +536,8 @@ double vpMatrix::detByLUOpenCV() const double det = 0.; if (rowNum != colNum) { - throw(vpException( - vpException::fatalError, - "Cannot compute matrix determinant of a non square matrix (%ux%u)", - rowNum, colNum)); + throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)", + rowNum, colNum)); } cv::Mat M(rowNum, colNum, CV_64F, this->data); @@ -607,18 +583,14 @@ inverseByLUGsl() vpMatrix vpMatrix::inverseByLUEigen3() const { if (rowNum != colNum) { - throw(vpException(vpException::fatalError, - "Cannot inverse a non square matrix (%ux%u) by LU", - rowNum, colNum)); + throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum)); } vpMatrix A(this->getRows(), this->getCols()); - Eigen::Map< - Eigen::Matrix > - M(this->data, this->getRows(), this->getCols()); - Eigen::Map< - Eigen::Matrix > - A_(A.data, this->getRows(), this->getCols()); + Eigen::Map > M(this->data, this->getRows(), + this->getCols()); + Eigen::Map > A_(A.data, this->getRows(), + this->getCols()); A_ = M.inverse(); @@ -645,8 +617,7 @@ int main() std::cout << "Initial matrix: \n" << A << std::endl; // Compute the determinant - std:: cout << "Determinant by LU decomposition (Eigen3): " << -A.detByLUEigen3() << std::endl; + std:: cout << "Determinant by LU decomposition (Eigen3): " << A.detByLUEigen3() << std::endl; } \endcode \sa detByLU(), detByLUOpenCV(), detByLULapack() @@ -654,15 +625,12 @@ A.detByLUEigen3() << std::endl; double vpMatrix::detByLUEigen3() const { if (rowNum != colNum) { - throw(vpException( - vpException::fatalError, - "Cannot compute matrix determinant of a non square matrix (%ux%u)", - rowNum, colNum)); + throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)", + rowNum, colNum)); } - Eigen::Map< - Eigen::Matrix > - M(this->data, this->getRows(), this->getCols()); + Eigen::Map > M(this->data, this->getRows(), + this->getCols()); return M.determinant(); } diff --git a/modules/core/src/math/matrix/vpMatrix_mul.cpp b/modules/core/src/math/matrix/vpMatrix_mul.cpp index 15f5edd49a..2b0193b515 100644 --- a/modules/core/src/math/matrix/vpMatrix_mul.cpp +++ b/modules/core/src/math/matrix/vpMatrix_mul.cpp @@ -41,38 +41,29 @@ #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) typedef int integer; -extern "C" void dgemm_(char *transa, char *transb, integer *M, integer *N, - integer *K, double *alpha, double *a, integer *lda, - double *b, integer *ldb, double *beta, double *c, - integer *ldc); +extern "C" void dgemm_(char *transa, char *transb, integer *M, integer *N, integer *K, double *alpha, double *a, + integer *lda, double *b, integer *ldb, double *beta, double *c, integer *ldc); -extern "C" void dgemv_(char *trans, integer *M, integer *N, double *alpha, - double *a, integer *lda, double *x, integer *incx, - double *beta, double *y, integer *incy); +extern "C" void dgemv_(char *trans, integer *M, integer *N, double *alpha, double *a, integer *lda, double *x, + integer *incx, double *beta, double *y, integer *incy); -void vpMatrix::blas_dgemm(char trans_a, char trans_b, const int M_, - const int N_, const int K_, double alpha, - double *a_data, const int lda_, double *b_data, - const int ldb_, double beta, double *c_data, +void vpMatrix::blas_dgemm(char trans_a, char trans_b, const int M_, const int N_, const int K_, double alpha, + double *a_data, const int lda_, double *b_data, const int ldb_, double beta, double *c_data, const int ldc_) { integer M = (integer)M_, K = (integer)K_, N = (integer)N_; integer lda = (integer)lda_, ldb = (integer)ldb_, ldc = (integer)ldc_; - dgemm_(&trans_a, &trans_b, &M, &N, &K, &alpha, a_data, &lda, b_data, &ldb, - &beta, c_data, &ldc); + dgemm_(&trans_a, &trans_b, &M, &N, &K, &alpha, a_data, &lda, b_data, &ldb, &beta, c_data, &ldc); } -void vpMatrix::blas_dgemv(char trans, const int M_, const int N_, - double alpha, double *a_data, const int lda_, - double *x_data, const int incx_, double beta, - double *y_data, const int incy_) +void vpMatrix::blas_dgemv(char trans, const int M_, const int N_, double alpha, double *a_data, const int lda_, + double *x_data, const int incx_, double beta, double *y_data, const int incy_) { integer M = (integer)M_, N = (integer)N_; integer lda = (integer)lda_, incx = (integer)incx_, incy = (integer)incy_; - dgemv_(&trans, &M, &N, &alpha, a_data, &lda, x_data, &incx, &beta, y_data, - &incy); + dgemv_(&trans, &M, &N, &alpha, a_data, &lda, x_data, &incx, &beta, y_data, &incy); } #else diff --git a/modules/core/src/math/matrix/vpMatrix_qr.cpp b/modules/core/src/math/matrix/vpMatrix_qr.cpp index 3c8427aa3e..4e54e52bdc 100644 --- a/modules/core/src/math/matrix/vpMatrix_qr.cpp +++ b/modules/core/src/math/matrix/vpMatrix_qr.cpp @@ -58,17 +58,12 @@ typedef long int integer; typedef int integer; #endif -extern "C" int dgeqrf_(integer *m, integer *n, double *a, integer *lda, - double *tau, double *work, integer *lwork, +extern "C" int dgeqrf_(integer *m, integer *n, double *a, integer *lda, double *tau, double *work, integer *lwork, integer *info); -extern "C" int dormqr_(char *side, char *trans, integer *m, integer *n, - integer *k, double *a, integer *lda, double *tau, - double *c__, integer *ldc, double *work, - integer *lwork, integer *info); -extern "C" int dorgqr_(integer *, integer *, integer *, double *, integer *, - double *, double *, integer *, integer *); -extern "C" int dtrtri_(char *uplo, char *diag, integer *n, double *a, - integer *lda, integer *info); +extern "C" int dormqr_(char *side, char *trans, integer *m, integer *n, integer *k, double *a, integer *lda, + double *tau, double *c__, integer *ldc, double *work, integer *lwork, integer *info); +extern "C" int dorgqr_(integer *, integer *, integer *, double *, integer *, double *, double *, integer *, integer *); +extern "C" int dtrtri_(char *uplo, char *diag, integer *n, double *a, integer *lda, integer *info); int allocate_work(double **work); @@ -114,15 +109,13 @@ int main() vpMatrix vpMatrix::inverseByQRLapack() const { if (rowNum != colNum) { - throw(vpMatrixException( - vpMatrixException::matrixError, - "Cannot inverse a non-square matrix (%ux%u) by QR", rowNum, colNum)); + throw(vpMatrixException(vpMatrixException::matrixError, "Cannot inverse a non-square matrix (%ux%u) by QR", rowNum, + colNum)); } integer rowNum_ = (integer)this->getRows(); integer colNum_ = (integer)this->getCols(); - integer lda = (integer) - rowNum_; // lda is the number of rows because we don't use a submatrix + integer lda = (integer)rowNum_; // lda is the number of rows because we don't use a submatrix integer dimTau = (std::min)(rowNum_, colNum_); integer dimWork = -1; double *tau = new double[dimTau]; @@ -143,18 +136,17 @@ vpMatrix vpMatrix::inverseByQRLapack() const 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)) + &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; + std::cout << "dgeqrf_:Preparation:" << -info << "th element had an illegal value" << std::endl; throw vpMatrixException::badValue; } dimWork = allocate_work(&work); @@ -169,18 +161,17 @@ vpMatrix vpMatrix::inverseByQRLapack() const 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)) + &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; + std::cout << "dgeqrf_:" << -info << "th element had an illegal value" << std::endl; throw vpMatrixException::badValue; } @@ -192,8 +183,7 @@ vpMatrix vpMatrix::inverseByQRLapack() const 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; + 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 " @@ -220,21 +210,19 @@ vpMatrix vpMatrix::inverseByQRLapack() const // 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); + 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; + 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); + 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; + std::cout << "dormqr_:" << -info << "th element had an illegal value" << std::endl; throw vpMatrixException::badValue; } delete[] tau; @@ -285,7 +273,6 @@ vpMatrix vpMatrix::inverseByQR() const #ifdef VISP_HAVE_LAPACK return inverseByQRLapack(); #else - throw(vpException(vpException::fatalError, - "Cannot inverse matrix by QR. Install Lapack 3rd party")); + throw(vpException(vpException::fatalError, "Cannot inverse matrix by QR. Install Lapack 3rd party")); #endif } diff --git a/modules/core/src/math/matrix/vpMatrix_svd.cpp b/modules/core/src/math/matrix/vpMatrix_svd.cpp index 22c67b11f0..06c33dc8bc 100644 --- a/modules/core/src/math/matrix/vpMatrix_svd.cpp +++ b/modules/core/src/math/matrix/vpMatrix_svd.cpp @@ -69,10 +69,8 @@ typedef long int integer; typedef int integer; #endif -extern "C" int dgesdd_(char *jobz, integer *m, integer *n, double *a, - integer *lda, double *s, double *u, integer *ldu, - double *vt, integer *ldvt, double *work, - integer *lwork, integer *iwork, integer *info); +extern "C" int dgesdd_(char *jobz, integer *m, integer *n, double *a, integer *lda, double *s, double *u, integer *ldu, + double *vt, integer *ldvt, double *work, integer *lwork, integer *iwork, integer *info); #include #include @@ -165,10 +163,8 @@ void vpMatrix::svdOpenCV(vpColVector &w, vpMatrix &V) 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)); + 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 @@ -264,18 +260,14 @@ void vpMatrix::svdLapack(vpColVector &w, vpMatrix &V) double *vt = this->data; lwork = -1; - dgesdd_((char *)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, &wkopt, &lwork, - iwork, &info); + dgesdd_((char *)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, &wkopt, &lwork, iwork, &info); lwork = (int)wkopt; work = new double[static_cast(lwork)]; - dgesdd_((char *)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work, &lwork, - iwork, &info); + dgesdd_((char *)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work, &lwork, iwork, &info); if (info > 0) { - throw( - vpMatrixException(vpMatrixException::fatalError, - "The algorithm computing SVD failed to converge.")); + throw(vpMatrixException(vpMatrixException::fatalError, "The algorithm computing SVD failed to converge.")); } V = V.transpose(); @@ -463,20 +455,16 @@ void vpMatrix::svdEigen3(vpColVector &w, vpMatrix &V) w.resize(this->getCols()); V.resize(this->getCols(), this->getCols()); - Eigen::Map< - Eigen::Matrix > - M(this->data, this->getRows(), this->getCols()); + Eigen::Map > M(this->data, this->getRows(), + this->getCols()); - Eigen::JacobiSVD svd(M, Eigen::ComputeThinU | - Eigen::ComputeThinV); + Eigen::JacobiSVD svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV); Eigen::Map w_(w.data, w.size()); - Eigen::Map< - Eigen::Matrix > - V_(V.data, V.getRows(), V.getCols()); - Eigen::Map< - Eigen::Matrix > - U_(this->data, this->getRows(), this->getCols()); + Eigen::Map > V_(V.data, V.getRows(), + V.getCols()); + Eigen::Map > U_(this->data, this->getRows(), + this->getCols()); w_ = svd.singularValues(); V_ = svd.matrixV(); U_ = svd.matrixU(); diff --git a/modules/core/src/math/matrix/vpRowVector.cpp b/modules/core/src/math/matrix/vpRowVector.cpp index 8fcb4f244f..0a027c124b 100644 --- a/modules/core/src/math/matrix/vpRowVector.cpp +++ b/modules/core/src/math/matrix/vpRowVector.cpp @@ -81,10 +81,8 @@ vpRowVector &vpRowVector::operator=(const vpRowVector &v) 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())); + 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) @@ -144,10 +142,8 @@ 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())); + throw(vpException(vpException::dimensionError, "Cannot multiply (1x%d) row vector by (%dx1) column vector", colNum, + x.getRows())); } double scalar = 0.0; @@ -177,9 +173,8 @@ 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())); + throw(vpException(vpException::dimensionError, "Cannot multiply (1x%d) row vector by (%dx%d) matrix", colNum, + M.getRows(), M.getCols())); } c = 0.0; @@ -335,10 +330,8 @@ vpRowVector vpRowVector::operator-() const 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())); + throw(vpException(vpException::dimensionError, "Cannot substract (1x%d) row vector to (1x%d) row vector", getCols(), + m.getCols())); } vpRowVector v(colNum); @@ -355,9 +348,8 @@ vpRowVector vpRowVector::operator-(const vpRowVector &m) const 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())); + throw(vpException(vpException::dimensionError, "Cannot add (1x%d) row vector to (1x%d) row vector", getCols(), + v.getCols())); } vpRowVector r(colNum); @@ -375,9 +367,8 @@ vpRowVector vpRowVector::operator+(const vpRowVector &v) const 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())); + 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++) @@ -393,10 +384,8 @@ vpRowVector &vpRowVector::operator+=(vpRowVector v) 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())); + 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++) @@ -456,8 +445,7 @@ 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(1, M.getCols()) +vpRowVector::vpRowVector(const vpMatrix &M, unsigned int i) : vpArray2D(1, M.getCols()) { for (unsigned int j = 0; j < M.getCols(); j++) (*this)[j] = M[i][j]; @@ -468,14 +456,11 @@ vpRowVector::vpRowVector(const vpMatrix &M, unsigned int i) \exception vpException::dimensionError If the matrix is not a 1-by-n matrix. */ -vpRowVector::vpRowVector(const vpMatrix &M) - : vpArray2D(1, M.getCols()) +vpRowVector::vpRowVector(const vpMatrix &M) : vpArray2D(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())); + 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++) @@ -485,8 +470,7 @@ vpRowVector::vpRowVector(const vpMatrix &M) /*! Constructor that creates a row vector from a std vector of double. */ -vpRowVector::vpRowVector(const std::vector &v) - : vpArray2D(1, (unsigned int)v.size()) +vpRowVector::vpRowVector(const std::vector &v) : vpArray2D(1, (unsigned int)v.size()) { for (unsigned int j = 0; j < v.size(); j++) (*this)[j] = v[j]; @@ -494,8 +478,7 @@ vpRowVector::vpRowVector(const std::vector &v) /*! Constructor that creates a row vector from a std vector of float. */ -vpRowVector::vpRowVector(const std::vector &v) - : vpArray2D(1, (unsigned int)v.size()) +vpRowVector::vpRowVector(const std::vector &v) : vpArray2D(1, (unsigned int)v.size()) { for (unsigned int j = 0; j < v.size(); j++) (*this)[j] = (double)(v[j]); @@ -514,9 +497,7 @@ vpRowVector::vpRowVector(const std::vector &v) \sa init() */ -vpRowVector::vpRowVector(const vpRowVector &v, unsigned int c, - unsigned int ncols) - : vpArray2D(1, ncols) +vpRowVector::vpRowVector(const vpRowVector &v, unsigned int c, unsigned int ncols) : vpArray2D(1, ncols) { init(v, c, ncols); } @@ -567,8 +548,7 @@ vpRowVector &vpRowVector::normalize() \sa reshape(vpMatrix &, const unsigned int &, const unsigned int &) */ -vpMatrix vpRowVector::reshape(const unsigned int &nrows, - const unsigned int &ncols) +vpMatrix vpRowVector::reshape(const unsigned int &nrows, const unsigned int &ncols) { vpMatrix M(nrows, ncols); reshape(M, nrows, ncols); @@ -618,13 +598,11 @@ row vector: 1 2 3 4 5 6 7 8 9 10 11 12 9 10 11 12 \endcode */ -void vpRowVector::reshape(vpMatrix &M, const unsigned int &nrows, - const unsigned int &ncols) +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())); + 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)) @@ -720,10 +698,7 @@ void vpRowVector::stack(const double &d) \sa stack(const vpRowVector &, const vpRowVector &, vpRowVector &) */ -void vpRowVector::stack(const vpRowVector &v) -{ - *this = vpRowVector::stack(*this, v); -} +void vpRowVector::stack(const vpRowVector &v) { *this = vpRowVector::stack(*this, v); } /*! Stack row vectors. @@ -772,8 +747,7 @@ vpRowVector vpRowVector::stack(const vpRowVector &A, const vpRowVector &B) \sa stack(const vpRowVector &) \sa stack(const vpRowVector &, const vpRowVector &) */ -void vpRowVector::stack(const vpRowVector &A, const vpRowVector &B, - vpRowVector &C) +void vpRowVector::stack(const vpRowVector &A, const vpRowVector &B, vpRowVector &C) { unsigned int nrA = A.getCols(); unsigned int nrB = B.getCols(); @@ -809,8 +783,7 @@ void vpRowVector::stack(const vpRowVector &A, const vpRowVector &B, double vpRowVector::mean(const vpRowVector &v) { if (v.data == NULL || v.size() == 0) { - throw(vpException(vpException::dimensionError, - "Cannot compute mean value of an empty row vector")); + throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector")); } double mean = 0; @@ -827,8 +800,7 @@ double vpRowVector::mean(const vpRowVector &v) double vpRowVector::median(const vpRowVector &v) { if (v.data == NULL || v.size() == 0) { - throw(vpException(vpException::dimensionError, - "Cannot compute mean value of an empty row vector")); + throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector")); } std::vector vectorOfDoubles(v.data, v.data + v.colNum); @@ -839,12 +811,10 @@ double vpRowVector::median(const vpRowVector &v) /*! Compute the standard deviation value of all the elements of the vector. */ -double vpRowVector::stdev(const vpRowVector &v, - const bool useBesselCorrection) +double vpRowVector::stdev(const vpRowVector &v, const bool useBesselCorrection) { if (v.data == NULL || v.size() == 0) { - throw(vpException(vpException::dimensionError, - "Cannot compute mean value of an empty row vector")); + throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector")); } double mean_value = mean(v); @@ -880,8 +850,7 @@ double vpRowVector::stdev(const vpRowVector &v, \sa std::ostream &operator<<(std::ostream &s, const vpArray2D &A) */ -int vpRowVector::print(std::ostream &s, unsigned int length, - char const *intro) const +int vpRowVector::print(std::ostream &s, unsigned int length, char const *intro) const { typedef std::string::size_type size_type; @@ -1059,16 +1028,13 @@ v: 0 1 2 3 w: 1 2 \endcode */ -void vpRowVector::init(const vpRowVector &v, unsigned int c, - unsigned int ncols) +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())); + 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 @@ -1106,23 +1072,17 @@ r[2] = 2; \endcode */ -std::ostream &vpRowVector::cppPrint(std::ostream &os, - const std::string &matrixName, - bool octet) const +std::ostream &vpRowVector::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const { - os << "vpRowVector " << matrixName << " (" << this->getCols() << "); " - << std::endl; + os << "vpRowVector " << matrixName << " (" << this->getCols() << "); " << std::endl; for (unsigned int j = 0; j < this->getCols(); ++j) { if (!octet) { - os << matrixName << "[" << j << "] = " << (*this)[j] << "; " - << std::endl; + os << matrixName << "[" << j << "] = " << (*this)[j] << "; " << std::endl; } else { for (unsigned int k = 0; k < sizeof(double); ++k) { - os << "((unsigned char*)&(" << matrixName << "[" << j << "]) )[" << k - << "] = 0x" << std::hex - << (unsigned int)((unsigned char *)&((*this)[j]))[k] << "; " - << std::endl; + os << "((unsigned char*)&(" << matrixName << "[" << j << "]) )[" << k << "] = 0x" << std::hex + << (unsigned int)((unsigned char *)&((*this)[j]))[k] << "; " << std::endl; } } } diff --git a/modules/core/src/math/matrix/vpSubColVector.cpp b/modules/core/src/math/matrix/vpSubColVector.cpp index d340abe2e0..95fc28aade 100644 --- a/modules/core/src/math/matrix/vpSubColVector.cpp +++ b/modules/core/src/math/matrix/vpSubColVector.cpp @@ -50,8 +50,7 @@ vpSubColVector::vpSubColVector() : vpColVector(), pRowNum(0), parent(NULL) {} \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) +vpSubColVector::vpSubColVector(vpColVector &v, const unsigned int &offset, const unsigned int &nrows) : vpColVector(), pRowNum(0), parent(NULL) { init(v, offset, nrows); @@ -63,8 +62,7 @@ vpSubColVector::vpSubColVector(vpColVector &v, const unsigned int &offset, \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) +void vpSubColVector::init(vpColVector &v, const unsigned int &offset, const unsigned int &nrows) { if (!v.data) { throw(vpException(vpException::fatalError, "Cannot initialize a " @@ -91,9 +89,8 @@ void vpSubColVector::init(vpColVector &v, const unsigned int &offset, dsize = rowNum; } else { - throw(vpException(vpException::dimensionError, - "Cannot create a sub-column vector that is not " - "completely containt in the parrent column vector")); + throw(vpException(vpException::dimensionError, "Cannot create a sub-column vector that is not " + "completely containt in the parrent column vector")); } } @@ -108,14 +105,10 @@ vpSubColVector::~vpSubColVector() { data = NULL; } void vpSubColVector::checkParentStatus() const { if (!data) { - throw(vpException( - vpException::fatalError, - "The parent of the current sub-column vector has been destroyed")); + 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")); + throw(vpException(vpException::dimensionError, "The size of the parent sub-column vector has changed")); } } @@ -168,10 +161,8 @@ vpSubColVector &vpSubColVector::operator=(const vpColVector &B) 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())); + 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++) diff --git a/modules/core/src/math/matrix/vpSubMatrix.cpp b/modules/core/src/math/matrix/vpSubMatrix.cpp index 53a450db36..7ed25dd93d 100644 --- a/modules/core/src/math/matrix/vpSubMatrix.cpp +++ b/modules/core/src/math/matrix/vpSubMatrix.cpp @@ -52,8 +52,7 @@ vpSubMatrix::vpSubMatrix() : pRowNum(0), pColNum(0), parent(NULL) {} \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, +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) { @@ -68,18 +67,15 @@ vpSubMatrix::vpSubMatrix(vpMatrix &m, const unsigned int &row_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, +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) { - throw(vpMatrixException(vpMatrixException::subMatrixError, - "SubMatrix parent matrix is not allocated")); + throw(vpMatrixException(vpMatrixException::subMatrixError, "SubMatrix parent matrix is not allocated")); } - if (row_offset + nrows <= m.getRows() && - col_offset + ncols <= m.getCols()) { + if (row_offset + nrows <= m.getRows() && col_offset + ncols <= m.getCols()) { data = m.data; parent = &m; rowNum = nrows; @@ -96,8 +92,8 @@ void vpSubMatrix::init(vpMatrix &m, const unsigned int &row_offset, dsize = pRowNum * pColNum; } else { - throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError, - "Submatrix cannot be contain in parent matrix")); + throw( + vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "Submatrix cannot be contain in parent matrix")); } } @@ -108,14 +104,12 @@ void vpSubMatrix::init(vpMatrix &m, const unsigned int &row_offset, void vpSubMatrix::checkParentStatus() const { if (!data) { - throw( - vpMatrixException(vpMatrixException::incorrectMatrixSizeError, - "vpSubMatrix parent vpMatrix has been destroyed")); + throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError, + "vpSubMatrix parent vpMatrix has been destroyed")); } if (pRowNum != parent->getRows() || pColNum != parent->getCols()) { - throw(vpMatrixException( - vpMatrixException::incorrectMatrixSizeError, - "vpSubMatrix size of parent vpMatrix has been changed")); + throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError, + "vpSubMatrix size of parent vpMatrix has been changed")); } } @@ -127,9 +121,8 @@ vpSubMatrix &vpSubMatrix::operator=(const vpMatrix &B) { if ((colNum != B.getCols()) || (rowNum != B.getRows())) { - throw(vpMatrixException( - vpMatrixException::incorrectMatrixSizeError, - "vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix")); + throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError, + "vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix")); } for (unsigned int i = 0; i < rowNum; i++) { @@ -148,9 +141,8 @@ vpSubMatrix &vpSubMatrix::operator=(const vpSubMatrix &B) { if ((colNum != B.getCols()) || (rowNum != B.getRows())) { - throw(vpMatrixException( - vpMatrixException::incorrectMatrixSizeError, - "vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix")); + throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError, + "vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix")); } pRowNum = B.pRowNum; diff --git a/modules/core/src/math/matrix/vpSubRowVector.cpp b/modules/core/src/math/matrix/vpSubRowVector.cpp index b62e86982d..c095759ce5 100644 --- a/modules/core/src/math/matrix/vpSubRowVector.cpp +++ b/modules/core/src/math/matrix/vpSubRowVector.cpp @@ -50,8 +50,7 @@ vpSubRowVector::vpSubRowVector() : vpRowVector(), pColNum(0), parent(NULL) {} \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) +vpSubRowVector::vpSubRowVector(vpRowVector &v, const unsigned int &offset, const unsigned int &ncols) : vpRowVector(), pColNum(0), parent(NULL) { init(v, offset, ncols); @@ -63,8 +62,7 @@ vpSubRowVector::vpSubRowVector(vpRowVector &v, const unsigned int &offset, \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) +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 " @@ -90,9 +88,8 @@ void vpSubRowVector::init(vpRowVector &v, const unsigned int &offset, dsize = colNum; } else { - throw(vpException(vpException::dimensionError, - "Cannot create a sub-row vector that is not completely " - "containt in the parrent row vector")); + throw(vpException(vpException::dimensionError, "Cannot create a sub-row vector that is not completely " + "containt in the parrent row vector")); } } @@ -107,13 +104,10 @@ vpSubRowVector::~vpSubRowVector() { data = NULL; } void vpSubRowVector::checkParentStatus() const { if (!data) { - throw(vpException( - vpException::fatalError, - "The parent of the current sub-row vector has been destroyed")); + 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")); + throw(vpException(vpException::dimensionError, "The size of the parent sub-row vector has changed")); } } @@ -126,10 +120,8 @@ void vpSubRowVector::checkParentStatus() const 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())); + throw(vpException(vpException::dimensionError, "Cannot initialize (1x%d) sub-row vector from (1x%d) sub-row vector", + colNum, B.getCols())); } pColNum = B.pColNum; parent = B.parent; @@ -148,10 +140,8 @@ vpSubRowVector &vpSubRowVector::operator=(const vpSubRowVector &B) 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())); + 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++) @@ -169,10 +159,8 @@ vpSubRowVector &vpSubRowVector::operator=(const vpRowVector &B) 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())); + 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++) diff --git a/modules/core/src/math/misc/vpHinkley.cpp b/modules/core/src/math/misc/vpHinkley.cpp index e7f0d7eea1..415e0db15e 100644 --- a/modules/core/src/math/misc/vpHinkley.cpp +++ b/modules/core/src/math/misc/vpHinkley.cpp @@ -73,10 +73,7 @@ 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) -{ -} +vpHinkley::vpHinkley() : dmin2(0.1), alpha(0.2), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0) {} /*! @@ -95,8 +92,7 @@ vpHinkley::vpHinkley() */ 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) + : dmin2(delta_val / 2.), alpha(alpha_val), nsignal(0), mean(0), Sk(0), Mk(0), Tk(0), Nk(0) { } @@ -190,8 +186,7 @@ vpHinkley::vpHinkleyJumpType vpHinkley::testDownwardJump(double signal) computeMk(); - vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 - << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk; + vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk; // teste si les variables cumulees excedent le seuil if ((Mk - Sk) > alpha) @@ -251,8 +246,7 @@ vpHinkley::vpHinkleyJumpType vpHinkley::testUpwardJump(double signal) computeNk(); - vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 - << " signal: " << signal << " Tk: " << Tk << " Nk: " << Nk; + vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Tk: " << Tk << " Nk: " << Nk; // teste si les variables cumulees excedent le seuil if ((Tk - Nk) > alpha) @@ -313,8 +307,7 @@ vpHinkley::vpHinkleyJumpType vpHinkley::testDownUpwardJump(double signal) computeMk(); computeNk(); - vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 - << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk + 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 @@ -373,10 +366,8 @@ void vpHinkley::computeMean(double signal) // 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::epsilon()) && - (std::fabs(Tk - Nk) <= std::fabs(vpMath::maximum(Tk, Nk)) * - std::numeric_limits::epsilon())) + if ((std::fabs(Mk - Sk) <= std::fabs(vpMath::maximum(Mk, Sk)) * std::numeric_limits::epsilon()) && + (std::fabs(Tk - Nk) <= std::fabs(vpMath::maximum(Tk, Nk)) * std::numeric_limits::epsilon())) // Fin modif FS le 03/09/2003 // Mise a jour de la moyenne. diff --git a/modules/core/src/math/misc/vpMath.cpp b/modules/core/src/math/misc/vpMath.cpp index 452eaf6296..f4364c22df 100644 --- a/modules/core/src/math/misc/vpMath.cpp +++ b/modules/core/src/math/misc/vpMath.cpp @@ -54,7 +54,7 @@ #include #endif -#if !(defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN)) || \ +#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; @@ -98,9 +98,7 @@ bool vpMath::isNaN(const double value) // Taken from OpenCV source code CvIsNan() Cv64suf ieee754; ieee754.f = value; - return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) + - ((unsigned)ieee754.u != 0) > - 0x7ff00000) != 0; + return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0) > 0x7ff00000) != 0; #endif #endif } @@ -121,8 +119,7 @@ bool vpMath::isInf(const double value) // Taken from OpenCV source code CvIsInf() Cv64suf ieee754; ieee754.f = value; - return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && - (unsigned)ieee754.u == 0; + return ((unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && (unsigned)ieee754.u == 0; #endif } @@ -252,8 +249,7 @@ double vpMath::getMedian(const std::vector &v) \return The standard deviation value. */ -double vpMath::getStdev(const std::vector &v, - const bool useBesselCorrection) +double vpMath::getStdev(const std::vector &v, const bool useBesselCorrection) { if (v.empty()) { throw vpException(vpException::notInitialized, "Empty vector !"); @@ -262,10 +258,8 @@ double vpMath::getStdev(const std::vector &v, double mean = getMean(v); std::vector diff(v.size()); - std::transform(v.begin(), v.end(), diff.begin(), - std::bind2nd(std::minus(), mean)); - double sq_sum = - std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); + std::transform(v.begin(), v.end(), diff.begin(), std::bind2nd(std::minus(), 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; diff --git a/modules/core/src/math/robust/vpRobust.cpp b/modules/core/src/math/robust/vpRobust.cpp index c8c1917c7f..45def4de34 100644 --- a/modules/core/src/math/robust/vpRobust.cpp +++ b/modules/core/src/math/robust/vpRobust.cpp @@ -63,8 +63,7 @@ */ vpRobust::vpRobust(unsigned int n_data) - : normres(), sorted_normres(), sorted_residues(), NoiseThreshold(0.0017), - sig_prev(0), it(0), swap(0), size(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; @@ -78,8 +77,7 @@ vpRobust::vpRobust(unsigned int n_data) Default constructor. */ vpRobust::vpRobust() - : normres(), sorted_normres(), sorted_residues(), NoiseThreshold(0.0017), - sig_prev(0), it(0), swap(0), size(0) + : normres(), sorted_normres(), sorted_residues(), NoiseThreshold(0.0017), sig_prev(0), it(0), swap(0), size(0) { } @@ -175,8 +173,7 @@ void vpRobust::resize(unsigned int n_data) */ // =================================================================== -void vpRobust::MEstimator(const vpRobustEstimatorType method, - const vpColVector &residues, vpColVector &weights) +void vpRobust::MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights) { double med = 0; // median @@ -192,8 +189,7 @@ void vpRobust::MEstimator(const vpRobustEstimatorType method, 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*/); + med = select(sorted_residues, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/); // residualMedian = med ; // Normalize residues @@ -203,8 +199,7 @@ void vpRobust::MEstimator(const vpRobustEstimatorType method, } // Calculate MAD - normmedian = select(sorted_normres, 0, (int)n_data - 1, - (int)ind_med /*(int)n_data/2*/); + 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 @@ -233,10 +228,8 @@ void vpRobust::MEstimator(const vpRobustEstimatorType method, } } -void vpRobust::MEstimator(const vpRobustEstimatorType method, - const vpColVector &residues, - const vpColVector &all_residues, - vpColVector &weights) +void vpRobust::MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, + const vpColVector &all_residues, vpColVector &weights) { double normmedian = 0; // Normalized median @@ -247,8 +240,7 @@ void vpRobust::MEstimator(const vpRobustEstimatorType method, // compute median with the residues vector, return all_normres which are the // normalized all_residues vector. - normmedian = - computeNormalizedMedian(all_normres, residues, all_residues, weights); + 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 @@ -277,10 +269,8 @@ void vpRobust::MEstimator(const vpRobustEstimatorType method, }; } -double vpRobust::computeNormalizedMedian(vpColVector &all_normres, - const vpColVector &residues, - const vpColVector &all_residues, - const vpColVector &weights) +double vpRobust::computeNormalizedMedian(vpColVector &all_normres, const vpColVector &residues, + const vpColVector &all_residues, const vpColVector &weights) { double med = 0; double normmedian = 0; @@ -309,20 +299,17 @@ double vpRobust::computeNormalizedMedian(vpColVector &all_normres, } } sorted_residues.resize(index); - memcpy(sorted_residues.data, no_null_weight_residues.data, - index * sizeof(double)); + 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; + 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*/); + med = select(sorted_residues, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/); unsigned int i; // Normalize residues @@ -337,8 +324,7 @@ double vpRobust::computeNormalizedMedian(vpColVector &all_normres, // normmedian = Median(normres, weights); // normmedian = Median(normres); - normmedian = select(sorted_normres, 0, (int)n_data - 1, - (int)ind_med /*(int)n_data/2*/); + normmedian = select(sorted_normres, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/); return normmedian; } @@ -362,8 +348,7 @@ vpColVector vpRobust::simultMEstimator(vpColVector &residues) vpColVector norm_res(n_data); // Normalized Residue vpColVector w(n_data); - vpCDEBUG(2) << "vpRobust MEstimator reached. No. data = " << n_data - << std::endl; + 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; @@ -377,9 +362,7 @@ vpColVector vpRobust::simultMEstimator(vpColVector &residues) // For Huber compute Simultaneous scale estimate // For Others use MAD calculated on first iteration if (it == 0) { - double normmedian = - select(norm_res, 0, (int)n_data - 1, - (int)ind_med /*(int)n_data/2*/); // Normalized Median + double normmedian = select(norm_res, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/); // Normalized Median // 1.48 keeps scale estimate consistent for a normal probability dist. sigma = 1.4826 * normmedian; // Median Absolute Deviation } else { @@ -434,16 +417,13 @@ double vpRobust::simultscale(vpColVector &x) #endif 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 << "Sum chi = " << chiTmp * vpMath::sqr(sig_prev) << std::endl; #if defined(VISP_HAVE_FUNC_STD_ERFC) - std::cout << "Expectation = " << chiTmp * std::erfc(chiTmp) - << std::endl; + std::cout << "Expectation = " << chiTmp * std::erfc(chiTmp) << std::endl; #elif defined(VISP_HAVE_FUNC_ERFC) std::cout << "Expectation = " << chiTmp * erfc(chiTmp) << std::endl; #else - std::cout << "Expectation = " << chiTmp * (1 - erf(chiTmp)) - << std::endl; + std::cout << "Expectation = " << chiTmp * (1 - erf(chiTmp)) << std::endl; #endif // getchar(); } @@ -491,12 +471,9 @@ double vpRobust::constrainedChiTukey(double x) double a = 4.7; // 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)); + 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; @@ -510,8 +487,7 @@ double vpRobust::constrainedChiCauchy(double x) double s = sig_prev; double b = 2.3849; - sct = - -1 * (vpMath::sqr(x) * b) / (s * (vpMath::sqr(s * b) + vpMath::sqr(x))); + sct = -1 * (vpMath::sqr(x) * b) / (s * (vpMath::sqr(s * b) + vpMath::sqr(x))); return sct; } @@ -572,8 +548,7 @@ void vpRobust::psiTukey(double sig, vpColVector &x, vpColVector &weights) 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::epsilon()) { + if ((std::fabs(xi_sig) <= (cst_const)) && std::fabs(weights[i]) > std::numeric_limits::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 { @@ -685,10 +660,7 @@ double vpRobust::select(vpColVector &a, int l, int r, int k) } #if !defined(VISP_HAVE_FUNC_ERFC) && !defined(VISP_HAVE_FUNC_STD_ERFC) -double vpRobust::erf(double x) -{ - return x < 0.0 ? -gammp(0.5, x * x) : gammp(0.5, x * x); -} +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) { @@ -763,8 +735,7 @@ void vpRobust::gcf(double *gammcf, double a, double x, double *gln) 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}; + 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; diff --git a/modules/core/src/math/robust/vpScale.cpp b/modules/core/src/math/robust/vpScale.cpp index 941ca2836c..3bb3a3c1a6 100644 --- a/modules/core/src/math/robust/vpScale.cpp +++ b/modules/core/src/math/robust/vpScale.cpp @@ -62,8 +62,7 @@ vpScale::vpScale() : bandwidth(0.02), dimension(1) } //! Constructor -vpScale::vpScale(double kernel_bandwidth, unsigned int dim) - : bandwidth(kernel_bandwidth), dimension(dim) +vpScale::vpScale(double kernel_bandwidth, unsigned int dim) : bandwidth(kernel_bandwidth), dimension(dim) { #if (DEBUG_LEVEL2) @@ -99,8 +98,7 @@ double vpScale::MeanShift(vpColVector &error) 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]); + mean_shift[i] = vpMath::sqr(bandwidth) * density_gradient[i] / ((dimension + 2) * density[i]); double tmp_shift = mean_shift[i]; @@ -148,8 +146,7 @@ double vpScale::KernelDensity(vpColVector &error, unsigned int position) j = position; // Then treat smaller errors // while(Ke !=0 && j>=dimension) - while (std::fabs(Ke) > std::numeric_limits::epsilon() && - j >= dimension) { + while (std::fabs(Ke) > std::numeric_limits::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; @@ -167,8 +164,7 @@ double vpScale::KernelDensity(vpColVector &error, unsigned int position) return density; } -double vpScale::KernelDensityGradient(vpColVector &error, - unsigned int position) +double vpScale::KernelDensityGradient(vpColVector &error, unsigned int position) { unsigned int n = error.getRows() / dimension; @@ -183,8 +179,7 @@ double vpScale::KernelDensityGradient(vpColVector &error, // the local density gradient // First treat larger errors than current // while(inside_kernel !=0 && j<=n) - while (std::fabs(inside_kernel) > std::numeric_limits::epsilon() && - j <= n) { + while (std::fabs(inside_kernel) > std::numeric_limits::epsilon() && j <= n) { delta = error[position] - error[j]; if (vpMath::sqr(delta / bandwidth) < 1) { inside_kernel = 1; @@ -199,8 +194,7 @@ double vpScale::KernelDensityGradient(vpColVector &error, j = position; // Then treat smaller errors than current // while(inside_kernel !=0 && j>=dimension) - while (std::fabs(inside_kernel) > std::numeric_limits::epsilon() && - j >= dimension) { + while (std::fabs(inside_kernel) > std::numeric_limits::epsilon() && j >= dimension) { delta = error[position] - error[j]; if (vpMath::sqr(delta / bandwidth) < 1) { inside_kernel = 1; @@ -234,8 +228,7 @@ double vpScale::KernelDensity_EPANECHNIKOV(vpColVector &X) c = 4 * M_PI / 3; break; default: - std::cout << "ERROR in vpScale::Kernel_EPANECHNIKOV : wrong dimension" - << std::endl; + std::cout << "ERROR in vpScale::Kernel_EPANECHNIKOV : wrong dimension" << std::endl; exit(1); } @@ -246,8 +239,7 @@ double vpScale::KernelDensity_EPANECHNIKOV(vpColVector &X) } // Epanechnikov_kernel for an d dimensional Euclidian space R^d -double vpScale::KernelDensityGradient_EPANECHNIKOV(double sumX, - unsigned int n) +double vpScale::KernelDensityGradient_EPANECHNIKOV(double sumX, unsigned int n) { double c; // Volume of an n dimensional unit sphere @@ -263,13 +255,11 @@ double vpScale::KernelDensityGradient_EPANECHNIKOV(double sumX, c = 4 * M_PI / 3; break; default: - std::cout << "ERROR in vpScale::Kernel_EPANECHNIKOV : wrong dimension" - << std::endl; + 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)); + 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 index 7953697a93..61b32cc38e 100644 --- a/modules/core/src/math/spline/vpBSpline.cpp +++ b/modules/core/src/math/spline/vpBSpline.cpp @@ -56,8 +56,7 @@ vpBSpline::vpBSpline() */ vpBSpline::vpBSpline(const vpBSpline &bspline) - : controlPoints(bspline.controlPoints), knots(bspline.knots), - p(bspline.p), // By default : p=3 for clubic spline + : controlPoints(bspline.controlPoints), knots(bspline.knots), p(bspline.p), // By default : p=3 for clubic spline crossingPoints(bspline.crossingPoints) { } @@ -82,8 +81,7 @@ vpBSpline::~vpBSpline() {} \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 &l_knots) +unsigned int vpBSpline::findSpan(double l_u, unsigned int l_p, std::vector &l_knots) { unsigned int m = (unsigned int)l_knots.size() - 1; @@ -95,8 +93,7 @@ unsigned int vpBSpline::findSpan(double l_u, unsigned int l_p, // 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::epsilon()) + std::fabs(vpMath::maximum(l_u, l_knots.back())) * std::numeric_limits::epsilon()) return ((unsigned int)(m - l_p - 1)); double low = l_p; @@ -148,8 +145,7 @@ unsigned int vpBSpline::findSpan(double u) { return findSpan(u, p, knots); } \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, +vpBasisFunction *vpBSpline::computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector &l_knots) { vpBasisFunction *N = new vpBasisFunction[l_p + 1]; @@ -235,10 +231,8 @@ vpBasisFunction *vpBSpline::computeBasisFuns(double u) 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 &l_knots) +vpBasisFunction **vpBSpline::computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, + std::vector &l_knots) { vpBasisFunction **N; N = new vpBasisFunction *[l_der + 1]; @@ -309,8 +303,7 @@ vpBSpline::computeDersBasisFuns(double l_u, unsigned int l_i, 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]; + 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]; } @@ -385,10 +378,8 @@ vpBasisFunction **vpBSpline::computeDersBasisFuns(double u, unsigned int der) 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 &l_knots, - std::vector &l_controlPoints) +vpImagePoint vpBSpline::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector &l_knots, + std::vector &l_controlPoints) { vpBasisFunction *N = computeBasisFuns(l_u, l_i, l_p, l_knots); vpImagePoint pt; @@ -457,10 +448,8 @@ vpImagePoint vpBSpline::computeCurvePoint(double 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 &l_knots, - std::vector &l_controlPoints) +vpImagePoint *vpBSpline::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, + std::vector &l_knots, std::vector &l_controlPoints) { vpImagePoint *derivate = new vpImagePoint[l_der + 1]; vpBasisFunction **N; @@ -476,12 +465,8 @@ vpBSpline::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, 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()); + 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()); } } @@ -525,12 +510,8 @@ vpImagePoint *vpBSpline::computeCurveDers(double u, unsigned int 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()); + 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()); } } diff --git a/modules/core/src/math/transformation/vpExponentialMap.cpp b/modules/core/src/math/transformation/vpExponentialMap.cpp index 6e50020bf2..43c14ef092 100644 --- a/modules/core/src/math/transformation/vpExponentialMap.cpp +++ b/modules/core/src/math/transformation/vpExponentialMap.cpp @@ -56,10 +56,7 @@ \sa inverse(const vpHomogeneousMatrix &) */ -vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v) -{ - return vpExponentialMap::direct(v, 1.0); -} +vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v) { return vpExponentialMap::direct(v, 1.0); } /*! @@ -79,8 +76,7 @@ vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v) \sa inverse(const vpHomogeneousMatrix &, const double &) */ -vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v, - const double &delta_t) +vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v, const double &delta_t) { double theta, si, co, sinc, mcosc, msinc; vpThetaUVector u; @@ -101,16 +97,13 @@ vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v, 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) + + 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) + + 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) + + 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; @@ -201,10 +194,7 @@ vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v, \sa direct(const vpColVector &) */ -vpColVector vpExponentialMap::inverse(const vpHomogeneousMatrix &M) -{ - return vpExponentialMap::inverse(M, 1.0); -} +vpColVector vpExponentialMap::inverse(const vpHomogeneousMatrix &M) { return vpExponentialMap::inverse(M, 1.0); } /*! @@ -223,8 +213,7 @@ vpColVector vpExponentialMap::inverse(const vpHomogeneousMatrix &M) \sa direct(const vpColVector &, const double &) */ -vpColVector vpExponentialMap::inverse(const vpHomogeneousMatrix &M, - const double &delta_t) +vpColVector vpExponentialMap::inverse(const vpHomogeneousMatrix &M, const double &delta_t) { vpColVector v(6); unsigned int i; @@ -261,22 +250,18 @@ vpColVector vpExponentialMap::inverse(const vpHomogeneousMatrix &M, 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]; + 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]) / + 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]) / + 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]) / + 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]; diff --git a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp index 21ee05be94..47e37c7a8b 100644 --- a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp +++ b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp @@ -95,11 +95,7 @@ vpForceTwistMatrix::vpForceTwistMatrix() : vpArray2D(6, 6) { eye(); } \param F : Force/torque twist matrix used as initializer. */ -vpForceTwistMatrix::vpForceTwistMatrix(const vpForceTwistMatrix &F) - : vpArray2D(6, 6) -{ - *this = F; -} +vpForceTwistMatrix::vpForceTwistMatrix(const vpForceTwistMatrix &F) : vpArray2D(6, 6) { *this = F; } /*! @@ -130,9 +126,7 @@ vpForceTwistMatrix::vpForceTwistMatrix(const vpForceTwistMatrix &F) \f] */ -vpForceTwistMatrix::vpForceTwistMatrix(const vpHomogeneousMatrix &M, - bool full) - : vpArray2D(6, 6) +vpForceTwistMatrix::vpForceTwistMatrix(const vpHomogeneousMatrix &M, bool full) : vpArray2D(6, 6) { if (full) buildFrom(M); @@ -159,8 +153,7 @@ vpForceTwistMatrix::vpForceTwistMatrix(const vpHomogeneousMatrix &M, \param thetau : \f$\theta u\f$ rotation vector used to initialize \f$R\f$. */ -vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &t, - const vpThetaUVector &thetau) +vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &t, const vpThetaUVector &thetau) : vpArray2D(6, 6) { buildFrom(t, thetau); @@ -183,11 +176,7 @@ vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &t, \param thetau : \f$\theta u\f$ rotation vector used to initialize \f$R\f$. */ -vpForceTwistMatrix::vpForceTwistMatrix(const vpThetaUVector &thetau) - : vpArray2D(6, 6) -{ - buildFrom(thetau); -} +vpForceTwistMatrix::vpForceTwistMatrix(const vpThetaUVector &thetau) : vpArray2D(6, 6) { buildFrom(thetau); } /*! @@ -208,8 +197,7 @@ vpForceTwistMatrix::vpForceTwistMatrix(const vpThetaUVector &thetau) \param R : Rotation matrix. */ -vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &t, - const vpRotationMatrix &R) +vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &t, const vpRotationMatrix &R) : vpArray2D(6, 6) { buildFrom(t, R); @@ -232,11 +220,7 @@ vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &t, \param R : Rotation matrix. */ -vpForceTwistMatrix::vpForceTwistMatrix(const vpRotationMatrix &R) - : vpArray2D(6, 6) -{ - buildFrom(R); -} +vpForceTwistMatrix::vpForceTwistMatrix(const vpRotationMatrix &R) : vpArray2D(6, 6) { buildFrom(R); } /*! @@ -258,8 +242,7 @@ vpForceTwistMatrix::vpForceTwistMatrix(const vpRotationMatrix &R) \param tux,tuy,tuz : \f$\theta {\bf u}\f$ rotation vector expressed in radians used to initialize \f$R\f$. */ -vpForceTwistMatrix::vpForceTwistMatrix(const double tx, const double ty, - const double tz, const double tux, +vpForceTwistMatrix::vpForceTwistMatrix(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz) : vpArray2D(6, 6) { @@ -286,8 +269,7 @@ int main() \endcode */ -vpForceTwistMatrix vpForceTwistMatrix:: -operator*(const vpForceTwistMatrix &F) const +vpForceTwistMatrix vpForceTwistMatrix::operator*(const vpForceTwistMatrix &F) const { vpForceTwistMatrix Fout; @@ -313,10 +295,8 @@ 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())); + 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()); @@ -364,8 +344,7 @@ int main() pMs[2][3] = -0.262; // tz only // Set the force/torque twist transformation - vpForceTwistMatrix pFs(pMs); // Twist transformation matrix from probe to -sensor frame + 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 @@ -420,8 +399,7 @@ vpColVector vpForceTwistMatrix::operator*(const vpColVector &H) const \param R : Rotation matrix. */ -vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpTranslationVector &t, - const vpRotationMatrix &R) +vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R) { vpMatrix skewaR = t.skew(t) * R; @@ -484,9 +462,7 @@ vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpRotationMatrix &R) \f$\bf R \f$. */ -vpForceTwistMatrix -vpForceTwistMatrix::buildFrom(const vpTranslationVector &tv, - const vpThetaUVector &thetau) +vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpTranslationVector &tv, const vpThetaUVector &thetau) { buildFrom(tv, vpRotationMatrix(thetau)); return (*this); @@ -544,8 +520,7 @@ vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpThetaUVector &thetau) \right] \f] */ -vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M, - bool full) +vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M, bool full) { if (full) buildFrom(M.getTranslationVector(), M.getRotationMatrix()); @@ -574,8 +549,7 @@ vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M, \sa std::ostream &operator <<(ostream &s,const vpMatrix &m) */ -int vpForceTwistMatrix::print(std::ostream &s, unsigned int length, - char const *intro) const +int vpForceTwistMatrix::print(std::ostream &s, unsigned int length, char const *intro) const { typedef std::string::size_type size_type; diff --git a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp index d78288e123..548f172bc1 100644 --- a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp +++ b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp @@ -53,8 +53,7 @@ Construct an homogeneous matrix from a translation vector and quaternion rotation vector. */ -vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, - const vpQuaternionVector &q) +vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, const vpQuaternionVector &q) : vpArray2D(4, 4) { buildFrom(t, q); @@ -64,27 +63,19 @@ vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, /*! Default constructor that initialize an homogeneous matrix as identity. */ -vpHomogeneousMatrix::vpHomogeneousMatrix() : vpArray2D(4, 4) -{ - eye(); -} +vpHomogeneousMatrix::vpHomogeneousMatrix() : vpArray2D(4, 4) { eye(); } /*! Copy constructor that initialize an homogeneous matrix from another homogeneous matrix. */ -vpHomogeneousMatrix::vpHomogeneousMatrix(const vpHomogeneousMatrix &M) - : vpArray2D(4, 4) -{ - *this = M; -} +vpHomogeneousMatrix::vpHomogeneousMatrix(const vpHomogeneousMatrix &M) : vpArray2D(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) +vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, const vpThetaUVector &tu) : vpArray2D(4, 4) { buildFrom(t, tu); @@ -95,8 +86,7 @@ vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, Construct an homogeneous matrix from a translation vector and a rotation matrix. */ -vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, - const vpRotationMatrix &R) +vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, const vpRotationMatrix &R) : vpArray2D(4, 4) { insert(R); @@ -107,8 +97,7 @@ vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, /*! Construct an homogeneous matrix from a pose vector. */ -vpHomogeneousMatrix::vpHomogeneousMatrix(const vpPoseVector &p) - : vpArray2D(4, 4) +vpHomogeneousMatrix::vpHomogeneousMatrix(const vpPoseVector &p) : vpArray2D(4, 4) { buildFrom(p[0], p[1], p[2], p[3], p[4], p[5]); (*this)[3][3] = 1.; @@ -153,8 +142,7 @@ v: 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 &v) - : vpArray2D(4, 4) +vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector &v) : vpArray2D(4, 4) { buildFrom(v); (*this)[3][3] = 1.; @@ -199,8 +187,7 @@ v: 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 &v) - : vpArray2D(4, 4) +vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector &v) : vpArray2D(4, 4) { buildFrom(v); (*this)[3][3] = 1.; @@ -211,8 +198,7 @@ vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector &v) 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, +vpHomogeneousMatrix::vpHomogeneousMatrix(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz) : vpArray2D(4, 4) { @@ -224,8 +210,7 @@ vpHomogeneousMatrix::vpHomogeneousMatrix(const double tx, const double ty, 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) +void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t, const vpThetaUVector &tu) { insert(tu); insert(t); @@ -235,8 +220,7 @@ void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t, Build an homogeneous matrix from a translation vector and a rotation matrix. */ -void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t, - const vpRotationMatrix &R) +void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R) { insert(R); insert(t); @@ -258,8 +242,7 @@ void vpHomogeneousMatrix::buildFrom(const vpPoseVector &p) Build an homogeneous matrix from a translation vector and a quaternion rotation vector. */ -void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t, - const vpQuaternionVector &q) +void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t, const vpQuaternionVector &q) { insert(t); insert(q); @@ -270,8 +253,7 @@ void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t, 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, +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); @@ -324,9 +306,7 @@ v: 0 -1 0 0.3 1 0 0 0.4 0 0 -1 0.5 void vpHomogeneousMatrix::buildFrom(const std::vector &v) { if (v.size() != 12 && v.size() != 16) { - throw(vpException( - vpException::dimensionError, - "Cannot convert std::vector to vpHomogeneousMatrix")); + throw(vpException(vpException::dimensionError, "Cannot convert std::vector to vpHomogeneousMatrix")); } for (unsigned int i = 0; i < 12; i++) @@ -376,9 +356,7 @@ v: 0 -1 0 0.3 1 0 0 0.4 0 0 -1 0.5 void vpHomogeneousMatrix::buildFrom(const std::vector &v) { if (v.size() != 12 && v.size() != 16) { - throw(vpException( - vpException::dimensionError, - "Cannot convert std::vector to vpHomogeneousMatrix")); + throw(vpException(vpException::dimensionError, "Cannot convert std::vector to vpHomogeneousMatrix")); } for (unsigned int i = 0; i < 12; i++) @@ -390,8 +368,7 @@ void vpHomogeneousMatrix::buildFrom(const std::vector &v) \param M : Matrix to copy. */ -vpHomogeneousMatrix &vpHomogeneousMatrix:: -operator=(const vpHomogeneousMatrix &M) +vpHomogeneousMatrix &vpHomogeneousMatrix::operator=(const vpHomogeneousMatrix &M) { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { @@ -418,8 +395,7 @@ int main() \endcode */ -vpHomogeneousMatrix vpHomogeneousMatrix:: -operator*(const vpHomogeneousMatrix &M) const +vpHomogeneousMatrix vpHomogeneousMatrix::operator*(const vpHomogeneousMatrix &M) const { vpHomogeneousMatrix p; @@ -459,8 +435,7 @@ int main() \endcode */ -vpHomogeneousMatrix &vpHomogeneousMatrix:: -operator*=(const vpHomogeneousMatrix &M) +vpHomogeneousMatrix &vpHomogeneousMatrix::operator*=(const vpHomogeneousMatrix &M) { (*this) = (*this) * M; return (*this); @@ -516,14 +491,10 @@ vpPoint vpHomogeneousMatrix::operator*(const vpPoint &bP) const 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[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]; @@ -551,16 +522,12 @@ vpPoint vpHomogeneousMatrix::operator*(const vpPoint &bP) const \return A translation vector that contains the new 3D coordinates after applying the homogeneous transformation. */ -vpTranslationVector vpHomogeneousMatrix:: -operator*(const vpTranslationVector &t) const +vpTranslationVector vpHomogeneousMatrix::operator*(const vpTranslationVector &t) const { vpTranslationVector t_out; - t_out[0] = (*this)[0][0] * t[0] + (*this)[0][1] * t[1] + - (*this)[0][2] * t[2] + (*this)[0][3]; - t_out[1] = (*this)[1][0] * t[0] + (*this)[1][1] * t[1] + - (*this)[1][2] * t[2] + (*this)[1][3]; - t_out[2] = (*this)[2][0] * t[0] + (*this)[2][1] * t[1] + - (*this)[2][2] * t[2] + (*this)[2][3]; + t_out[0] = (*this)[0][0] * t[0] + (*this)[0][1] * t[1] + (*this)[0][2] * t[2] + (*this)[0][3]; + t_out[1] = (*this)[1][0] * t[0] + (*this)[1][1] * t[1] + (*this)[1][2] * t[2] + (*this)[1][3]; + t_out[2] = (*this)[2][0] * t[0] + (*this)[2][1] * t[1] + (*this)[2][2] * t[2] + (*this)[2][3]; return t_out; } @@ -657,10 +624,7 @@ void vpHomogeneousMatrix::insert(const vpTranslationVector &t) quaternion rotation vector. */ -void vpHomogeneousMatrix::insert(const vpQuaternionVector &q) -{ - insert(vpRotationMatrix(q)); -} +void vpHomogeneousMatrix::insert(const vpQuaternionVector &q) { insert(vpRotationMatrix(q)); } /*! Invert the homogeneous matrix @@ -724,10 +688,7 @@ void vpHomogeneousMatrix::eye() \right]\f$ */ -void vpHomogeneousMatrix::inverse(vpHomogeneousMatrix &M) const -{ - M = inverse(); -} +void vpHomogeneousMatrix::inverse(vpHomogeneousMatrix &M) const { M = inverse(); } /*! Write an homogeneous matrix in an output file stream. @@ -756,8 +717,7 @@ void vpHomogeneousMatrix::save(std::ofstream &f) const if (!f.fail()) { f << *this; } else { - throw(vpException(vpException::ioError, - "Cannot save homogeneous matrix: ostream not open")); + throw(vpException(vpException::ioError, "Cannot save homogeneous matrix: ostream not open")); } } @@ -788,8 +748,7 @@ void vpHomogeneousMatrix::load(std::ifstream &f) } } } else { - throw(vpException(vpException::ioError, - "Cannot laad homogeneous matrix: ifstream not open")); + throw(vpException(vpException::ioError, "Cannot laad homogeneous matrix: ifstream not open")); } } @@ -884,9 +843,7 @@ Last column: 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")); + 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++) diff --git a/modules/core/src/math/transformation/vpPoseVector.cpp b/modules/core/src/math/transformation/vpPoseVector.cpp index 59c66fc75d..8dfbf27a0c 100644 --- a/modules/core/src/math/transformation/vpPoseVector.cpp +++ b/modules/core/src/math/transformation/vpPoseVector.cpp @@ -80,8 +80,7 @@ vpPoseVector::vpPoseVector() : vpArray2D(6, 1) {} 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, +vpPoseVector::vpPoseVector(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz) : vpArray2D(6, 1) { @@ -104,9 +103,7 @@ vpPoseVector::vpPoseVector(const double tx, const double ty, const double tz, \param tu : \f$\theta \bf u\f$ rotation vector. */ -vpPoseVector::vpPoseVector(const vpTranslationVector &tv, - const vpThetaUVector &tu) - : vpArray2D(6, 1) +vpPoseVector::vpPoseVector(const vpTranslationVector &tv, const vpThetaUVector &tu) : vpArray2D(6, 1) { buildFrom(tv, tu); } @@ -123,9 +120,7 @@ vpPoseVector::vpPoseVector(const vpTranslationVector &tv, u\f$ vector is extracted to initialise the pose vector. */ -vpPoseVector::vpPoseVector(const vpTranslationVector &tv, - const vpRotationMatrix &R) - : vpArray2D(6, 1) +vpPoseVector::vpPoseVector(const vpTranslationVector &tv, const vpRotationMatrix &R) : vpArray2D(6, 1) { buildFrom(tv, R); } @@ -140,11 +135,7 @@ vpPoseVector::vpPoseVector(const vpTranslationVector &tv, initialize the pose vector. */ -vpPoseVector::vpPoseVector(const vpHomogeneousMatrix &M) - : vpArray2D(6, 1) -{ - buildFrom(M); -} +vpPoseVector::vpPoseVector(const vpHomogeneousMatrix &M) : vpArray2D(6, 1) { buildFrom(M); } /*! @@ -161,8 +152,8 @@ vpPoseVector::vpPoseVector(const vpHomogeneousMatrix &M) 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) +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; @@ -189,8 +180,7 @@ void vpPoseVector::set(const double tx, const double ty, const double tz, \sa set() */ -vpPoseVector vpPoseVector::buildFrom(const double tx, const double ty, - const double tz, const double tux, +vpPoseVector vpPoseVector::buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz) { (*this)[0] = tx; @@ -235,8 +225,7 @@ vpPoseVector vpPoseVector::buildFrom(const vpHomogeneousMatrix &M) \return The build pose vector. */ -vpPoseVector vpPoseVector::buildFrom(const vpTranslationVector &tv, - const vpThetaUVector &tu) +vpPoseVector vpPoseVector::buildFrom(const vpTranslationVector &tv, const vpThetaUVector &tu) { for (unsigned int i = 0; i < 3; i++) { (*this)[i] = tv[i]; @@ -258,8 +247,7 @@ vpPoseVector vpPoseVector::buildFrom(const vpTranslationVector &tv, \return The build pose vector. */ -vpPoseVector vpPoseVector::buildFrom(const vpTranslationVector &tv, - const vpRotationMatrix &R) +vpPoseVector vpPoseVector::buildFrom(const vpTranslationVector &tv, const vpRotationMatrix &R) { vpThetaUVector tu; tu.buildFrom(R); @@ -298,10 +286,7 @@ void vpPoseVector::extract(vpQuaternionVector &q) const /*! Extract the rotation as a rotation matrix. */ -void vpPoseVector::extract(vpRotationMatrix &R) const -{ - R.buildFrom((*this)[3], (*this)[4], (*this)[5]); -} +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. @@ -379,8 +364,7 @@ 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")); + throw(vpException(vpException::ioError, "Cannot save the pose vector: ofstream not openned")); } } @@ -401,8 +385,7 @@ void vpPoseVector::load(std::ifstream &f) f >> (*this)[i]; } } else { - throw(vpException(vpException::ioError, - "Cannot read pose vector: ifstream not openned")); + throw(vpException(vpException::ioError, "Cannot read pose vector: ifstream not openned")); } } @@ -436,8 +419,7 @@ vpRowVector vpPoseVector::t() const \sa std::ostream &operator<<(std::ostream &s, const vpArray2D &A) */ -int vpPoseVector::print(std::ostream &s, unsigned int length, - char const *intro) const +int vpPoseVector::print(std::ostream &s, unsigned int length, char const *intro) const { typedef std::string::size_type size_type; diff --git a/modules/core/src/math/transformation/vpQuaternionVector.cpp b/modules/core/src/math/transformation/vpQuaternionVector.cpp index aaa63f0632..1426c3063b 100644 --- a/modules/core/src/math/transformation/vpQuaternionVector.cpp +++ b/modules/core/src/math/transformation/vpQuaternionVector.cpp @@ -54,28 +54,21 @@ const double vpQuaternionVector::minimum = 0.0001; vpQuaternionVector::vpQuaternionVector() : vpRotationVector(4) {} /*! Copy constructor. */ -vpQuaternionVector::vpQuaternionVector(const vpQuaternionVector &q) - : vpRotationVector(q) -{ -} +vpQuaternionVector::vpQuaternionVector(const vpQuaternionVector &q) : vpRotationVector(q) {} //! Constructor from doubles. -vpQuaternionVector::vpQuaternionVector(const double x_, const double y_, - const double z_, const double w_) +vpQuaternionVector::vpQuaternionVector(const double x_, const double y_, const double z_, const double w_) : vpRotationVector(4) { set(x_, y_, z_, w_); } //! Constructor from a 4-dimension vector of doubles. -vpQuaternionVector::vpQuaternionVector(const vpColVector &q) - : vpRotationVector(4) +vpQuaternionVector::vpQuaternionVector(const vpColVector &q) : vpRotationVector(4) { if (q.size() != 4) { - throw(vpException( - vpException::dimensionError, - "Cannot construct a quaternion vector from a %d-dimension col vector", - q.size())); + throw(vpException(vpException::dimensionError, + "Cannot construct a quaternion vector from a %d-dimension col vector", q.size())); } for (unsigned int i = 0; i < 4; i++) data[i] = q[i]; @@ -86,11 +79,7 @@ vpQuaternionVector::vpQuaternionVector(const vpColVector &q) \param R : Matrix containing a rotation. */ -vpQuaternionVector::vpQuaternionVector(const vpRotationMatrix &R) - : vpRotationVector(4) -{ - buildFrom(R); -} +vpQuaternionVector::vpQuaternionVector(const vpRotationMatrix &R) : vpRotationVector(4) { buildFrom(R); } /*! Constructor that initialize \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler @@ -98,11 +87,7 @@ vpQuaternionVector::vpQuaternionVector(const vpRotationMatrix &R) \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); -} +vpQuaternionVector::vpQuaternionVector(const vpThetaUVector &tu) : vpRotationVector(4) { buildFrom(tu); } /*! Manually change values of a quaternion. @@ -111,8 +96,7 @@ vpQuaternionVector::vpQuaternionVector(const vpThetaUVector &tu) \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) +void vpQuaternionVector::set(const double qx, const double qy, const double qz, const double qw) { data[0] = qx; data[1] = qy; @@ -128,10 +112,7 @@ void vpQuaternionVector::set(const double qx, const double qy, \sa set() */ -vpQuaternionVector vpQuaternionVector::buildFrom(const double qx, - const double qy, - const double qz, - const double qw) +vpQuaternionVector vpQuaternionVector::buildFrom(const double qx, const double qy, const double qz, const double qw) { set(qx, qy, qz, qw); return *this; @@ -157,11 +138,9 @@ vpQuaternionVector vpQuaternionVector::buildFrom(const vpThetaUVector &tu) \param q : quaternion to add. */ -vpQuaternionVector vpQuaternionVector:: -operator+(const vpQuaternionVector &q) const +vpQuaternionVector vpQuaternionVector::operator+(const vpQuaternionVector &q) const { - return vpQuaternionVector(x() + q.x(), y() + q.y(), z() + q.z(), - w() + q.w()); + return vpQuaternionVector(x() + q.x(), y() + q.y(), z() + q.z(), w() + q.w()); } /*! Quaternion substraction. @@ -170,18 +149,13 @@ operator+(const vpQuaternionVector &q) const \param q : quaternion to substract. */ -vpQuaternionVector vpQuaternionVector:: -operator-(const vpQuaternionVector &q) const +vpQuaternionVector vpQuaternionVector::operator-(const vpQuaternionVector &q) const { - return vpQuaternionVector(x() - q.x(), y() - q.y(), z() - q.z(), - w() - q.w()); + 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()); -} +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 @@ -190,14 +164,12 @@ vpQuaternionVector vpQuaternionVector::operator*(const double l) const } //! Multiply two quaternions. -vpQuaternionVector vpQuaternionVector:: -operator*(const vpQuaternionVector &rq) const +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()); + 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). @@ -236,10 +208,8 @@ int main() vpQuaternionVector &vpQuaternionVector::operator=(const vpColVector &q) { if (q.size() != 4) { - throw(vpException( - vpException::dimensionError, - "Cannot set a quaternion vector from a %d-dimension col vector", - q.size())); + throw(vpException(vpException::dimensionError, "Cannot set a quaternion vector from a %d-dimension col vector", + q.size())); } for (unsigned int i = 0; i < 4; i++) data[i] = q[i]; @@ -271,10 +241,7 @@ vpQuaternionVector vpQuaternionVector::buildFrom(const vpRotationMatrix &R) \return The conjugate quaternion. */ -vpQuaternionVector vpQuaternionVector::conjugate() const -{ - return vpQuaternionVector(-x(), -y(), -z(), w()); -} +vpQuaternionVector vpQuaternionVector::conjugate() const { return vpQuaternionVector(-x(), -y(), -z(), w()); } /*! Quaternion inverse. @@ -289,9 +256,7 @@ vpQuaternionVector vpQuaternionVector::inverse() const if (!vpMath::nul(mag_square, std::numeric_limits::epsilon())) { q_inv = this->conjugate() / mag_square; } else { - std::cerr - << "The current quaternion is null ! The inverse cannot be computed !" - << std::endl; + std::cerr << "The current quaternion is null ! The inverse cannot be computed !" << std::endl; } return q_inv; @@ -302,10 +267,7 @@ vpQuaternionVector vpQuaternionVector::inverse() const \return The magnitude or norm of the quaternion. */ -double vpQuaternionVector::magnitude() const -{ - return sqrt(w() * w() + x() * x() + y() * y() + z() * z()); -} +double vpQuaternionVector::magnitude() const { return sqrt(w() * w() + x() * x() + y() * y() + z() * z()); } /*! Normalize the quaternion. diff --git a/modules/core/src/math/transformation/vpRotationMatrix.cpp b/modules/core/src/math/transformation/vpRotationMatrix.cpp index 2b413e6703..172e54fa4f 100644 --- a/modules/core/src/math/transformation/vpRotationMatrix.cpp +++ b/modules/core/src/math/transformation/vpRotationMatrix.cpp @@ -112,10 +112,8 @@ vpRotationMatrix &vpRotationMatrix::operator=(const vpRotationMatrix &R) 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())); + 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++) { @@ -167,10 +165,8 @@ vpRotationMatrix vpRotationMatrix::operator*(const vpRotationMatrix &R) const 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())); + throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix", + M.getRows(), M.getCols())); } vpMatrix p(3, 3); @@ -238,8 +234,7 @@ vpColVector vpRotationMatrix::operator*(const vpColVector &v) const Multiply a rotation matrix by a translation vector and return the resulting translation vector. */ -vpTranslationVector vpRotationMatrix:: -operator*(const vpTranslationVector &tv) const +vpTranslationVector vpRotationMatrix::operator*(const vpTranslationVector &tv) const { vpTranslationVector p; @@ -309,17 +304,13 @@ bool vpRotationMatrix::isARotationMatrix() const // 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) + 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) + if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][2])) - 1) > threshold) isRotation = false; } @@ -336,76 +327,46 @@ vpRotationMatrix::vpRotationMatrix() : vpArray2D(3, 3) { eye(); } Copy contructor that construct a 3-by-3 rotation matrix from another rotation matrix. */ -vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) - : vpArray2D(3, 3) -{ - (*this) = M; -} +vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D(3, 3) { (*this) = M; } /*! Construct a 3-by-3 rotation matrix from an homogeneous matrix. */ -vpRotationMatrix::vpRotationMatrix(const vpHomogeneousMatrix &M) - : vpArray2D(3, 3) -{ - buildFrom(M); -} +vpRotationMatrix::vpRotationMatrix(const vpHomogeneousMatrix &M) : vpArray2D(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(3, 3) -{ - buildFrom(tu); -} +vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpArray2D(3, 3) { buildFrom(tu); } /*! Construct a 3-by-3 rotation matrix from a pose vector. */ -vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) - : vpArray2D(3, 3) -{ - buildFrom(p); -} +vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpArray2D(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(3, 3) -{ - buildFrom(euler); -} +vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D(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(3, 3) -{ - buildFrom(Rxyz); -} +vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D(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(3, 3) -{ - buildFrom(Rzyx); -} +vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D(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(3, 3) +vpRotationMatrix::vpRotationMatrix(const double tux, const double tuy, const double tuz) : vpArray2D(3, 3) { buildFrom(tux, tuy, tuz); } @@ -413,11 +374,7 @@ vpRotationMatrix::vpRotationMatrix(const double tux, const double tuy, /*! Construct a 3-by-3 rotation matrix from quaternion angle representation. */ -vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector &q) - : vpArray2D(3, 3) -{ - buildFrom(q); -} +vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector &q) : vpArray2D(3, 3) { buildFrom(q); } /*! Return the rotation matrix transpose which is also the inverse of the @@ -643,9 +600,7 @@ vpRotationMatrix vpRotationMatrix::buildFrom(const vpRzyxVector &v) 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) +vpRotationMatrix vpRotationMatrix::buildFrom(const double tux, const double tuy, const double tuz) { vpThetaUVector tu(tux, tuy, tuz); buildFrom(tu); @@ -733,9 +688,7 @@ Last column: 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")); + 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++) diff --git a/modules/core/src/math/transformation/vpRxyzVector.cpp b/modules/core/src/math/transformation/vpRxyzVector.cpp index 245385911d..c84bc29245 100644 --- a/modules/core/src/math/transformation/vpRxyzVector.cpp +++ b/modules/core/src/math/transformation/vpRxyzVector.cpp @@ -52,9 +52,7 @@ vpRxyzVector::vpRxyzVector() : vpRotationVector(3) {} /*! Copy constructor. */ -vpRxyzVector::vpRxyzVector(const vpRxyzVector &rxyz) : vpRotationVector(rxyz) -{ -} +vpRxyzVector::vpRxyzVector(const vpRxyzVector &rxyz) : vpRotationVector(rxyz) {} /*! Constructor from 3 angles (in radian). @@ -62,9 +60,7 @@ vpRxyzVector::vpRxyzVector(const vpRxyzVector &rxyz) : vpRotationVector(rxyz) \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) +vpRxyzVector::vpRxyzVector(const double phi, const double theta, const double psi) : vpRotationVector(3) { buildFrom(phi, theta, psi); } @@ -74,10 +70,7 @@ vpRxyzVector::vpRxyzVector(const double phi, const double theta, angles from a rotation matrix. \param R : Rotation matrix used to initialize the Euler angles. */ -vpRxyzVector::vpRxyzVector(const vpRotationMatrix &R) : vpRotationVector(3) -{ - buildFrom(R); -} +vpRxyzVector::vpRxyzVector(const vpRotationMatrix &R) : vpRotationVector(3) { buildFrom(R); } /*! Constructor that initialize \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler @@ -85,19 +78,14 @@ vpRxyzVector::vpRxyzVector(const vpRotationMatrix &R) : vpRotationVector(3) \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); -} +vpRxyzVector::vpRxyzVector(const vpThetaUVector &tu) : vpRotationVector(3) { buildFrom(tu); } /*! Copy constructor from a 3-dimension vector. */ vpRxyzVector::vpRxyzVector(const vpColVector &rxyz) : vpRotationVector(3) { if (rxyz.size() != 3) { - throw(vpException( - vpException::dimensionError, - "Cannot construct a R-xyz vector from a %d-dimension col vector", - rxyz.size())); + throw(vpException(vpException::dimensionError, "Cannot construct a R-xyz vector from a %d-dimension col vector", + rxyz.size())); } for (unsigned int i = 0; i < 3; i++) data[i] = rxyz[i]; @@ -123,8 +111,7 @@ vpRxyzVector vpRxyzVector::buildFrom(const vpRotationMatrix &R) double si = sin(phi); double co = cos(phi); double theta = atan2(R[0][2], -si * R[1][2] + co * R[2][2]); - double psi = - atan2(co * R[1][0] + si * R[2][0], co * R[1][1] + si * R[2][1]); + double psi = atan2(co * R[1][0] + si * R[2][0], co * R[1][1] + si * R[2][1]); buildFrom(phi, theta, psi); @@ -152,8 +139,7 @@ vpRxyzVector vpRxyzVector::buildFrom(const vpThetaUVector &tu) \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) +void vpRxyzVector::buildFrom(const double phi, const double theta, const double psi) { data[0] = phi; data[1] = theta; @@ -213,10 +199,8 @@ int main() vpRxyzVector &vpRxyzVector::operator=(const vpColVector &rxyz) { if (rxyz.size() != 3) { - throw(vpException( - vpException::dimensionError, - "Cannot set a R-xyz vector from a %d-dimension col vector", - rxyz.size())); + throw(vpException(vpException::dimensionError, "Cannot set a R-xyz vector from a %d-dimension col vector", + rxyz.size())); } for (unsigned int i = 0; i < 3; i++) data[i] = rxyz[i]; diff --git a/modules/core/src/math/transformation/vpRzyxVector.cpp b/modules/core/src/math/transformation/vpRzyxVector.cpp index f201c62850..b7a9475c5c 100644 --- a/modules/core/src/math/transformation/vpRzyxVector.cpp +++ b/modules/core/src/math/transformation/vpRzyxVector.cpp @@ -51,9 +51,7 @@ vpRzyxVector::vpRzyxVector() : vpRotationVector(3) {} /*! Copy constructor. */ -vpRzyxVector::vpRzyxVector(const vpRzyxVector &rzyx) : vpRotationVector(rzyx) -{ -} +vpRzyxVector::vpRzyxVector(const vpRzyxVector &rzyx) : vpRotationVector(rzyx) {} /*! Constructor from 3 angles (in radian). @@ -61,9 +59,7 @@ vpRzyxVector::vpRzyxVector(const vpRzyxVector &rzyx) : vpRotationVector(rzyx) \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) +vpRzyxVector::vpRzyxVector(const double phi, const double theta, const double psi) : vpRotationVector(3) { buildFrom(phi, theta, psi); } @@ -73,10 +69,7 @@ vpRzyxVector::vpRzyxVector(const double phi, const double theta, angles from a rotation matrix. \param R : Rotation matrix used to initialize the Euler angles. */ -vpRzyxVector::vpRzyxVector(const vpRotationMatrix &R) : vpRotationVector(3) -{ - buildFrom(R); -} +vpRzyxVector::vpRzyxVector(const vpRotationMatrix &R) : vpRotationVector(3) { buildFrom(R); } /*! Constructor that initialize \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler @@ -84,19 +77,14 @@ vpRzyxVector::vpRzyxVector(const vpRotationMatrix &R) : vpRotationVector(3) \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); -} +vpRzyxVector::vpRzyxVector(const vpThetaUVector &tu) : vpRotationVector(3) { buildFrom(tu); } /*! Copy constructor from a 3-dimension vector. */ vpRzyxVector::vpRzyxVector(const vpColVector &rzyx) : vpRotationVector(3) { if (rzyx.size() != 3) { - throw(vpException( - vpException::dimensionError, - "Cannot construct a R-zyx vector from a %d-dimension col vector", - rzyx.size())); + throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension col vector", + rzyx.size())); } for (unsigned int i = 0; i < 3; i++) data[i] = rzyx[i]; @@ -157,8 +145,7 @@ vpRzyxVector vpRzyxVector::buildFrom(const vpThetaUVector &tu) \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) +void vpRzyxVector::buildFrom(const double phi, const double theta, const double psi) { data[0] = phi; data[1] = theta; @@ -218,10 +205,8 @@ int main() vpRzyxVector &vpRzyxVector::operator=(const vpColVector &rzyx) { if (rzyx.size() != 3) { - throw(vpException( - vpException::dimensionError, - "Cannot set a R-zyx vector from a %d-dimension col vector", - rzyx.size())); + throw(vpException(vpException::dimensionError, "Cannot set a R-zyx vector from a %d-dimension col vector", + rzyx.size())); } for (unsigned int i = 0; i < 3; i++) data[i] = rzyx[i]; diff --git a/modules/core/src/math/transformation/vpRzyzVector.cpp b/modules/core/src/math/transformation/vpRzyzVector.cpp index 30bfa0318f..e2a5b66900 100644 --- a/modules/core/src/math/transformation/vpRzyzVector.cpp +++ b/modules/core/src/math/transformation/vpRzyzVector.cpp @@ -50,9 +50,7 @@ /*! Default constructor that initialize all the 3 angles to zero. */ vpRzyzVector::vpRzyzVector() : vpRotationVector(3) {} /*! Copy constructor. */ -vpRzyzVector::vpRzyzVector(const vpRzyzVector &rzyz) : vpRotationVector(rzyz) -{ -} +vpRzyzVector::vpRzyzVector(const vpRzyzVector &rzyz) : vpRotationVector(rzyz) {} /*! Constructor from 3 angles (in radian). @@ -60,9 +58,7 @@ vpRzyzVector::vpRzyzVector(const vpRzyzVector &rzyz) : vpRotationVector(rzyz) \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) +vpRzyzVector::vpRzyzVector(const double phi, const double theta, const double psi) : vpRotationVector(3) { buildFrom(phi, theta, psi); } @@ -72,10 +68,7 @@ vpRzyzVector::vpRzyzVector(const double phi, const double theta, angles from a rotation matrix. \param R : Rotation matrix used to initialize the Euler angles. */ -vpRzyzVector::vpRzyzVector(const vpRotationMatrix &R) : vpRotationVector(3) -{ - buildFrom(R); -} +vpRzyzVector::vpRzyzVector(const vpRotationMatrix &R) : vpRotationVector(3) { buildFrom(R); } /*! Constructor that initialize \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler @@ -83,19 +76,14 @@ vpRzyzVector::vpRzyzVector(const vpRotationMatrix &R) : vpRotationVector(3) \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); -} +vpRzyzVector::vpRzyzVector(const vpThetaUVector &tu) : vpRotationVector(3) { buildFrom(tu); } /*! Copy constructor from a 3-dimension vector. */ vpRzyzVector::vpRzyzVector(const vpColVector &rzyz) : vpRotationVector(3) { if (rzyz.size() != 3) { - throw(vpException( - vpException::dimensionError, - "Cannot construct a R-zyz vector from a %d-dimension col vector", - rzyz.size())); + throw(vpException(vpException::dimensionError, "Cannot construct a R-zyz vector from a %d-dimension col vector", + rzyz.size())); } for (unsigned int i = 0; i < 3; i++) data[i] = rzyz[i]; @@ -120,8 +108,7 @@ vpRzyzVector vpRzyzVector::buildFrom(const vpRotationMatrix &R) 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]); + double psi = atan2(-sphi * R[0][0] + cphi * R[1][0], -sphi * R[0][1] + cphi * R[1][1]); buildFrom(phi, theta, psi); @@ -176,8 +163,7 @@ vpRzyzVector &vpRzyzVector::operator=(double v) \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) +void vpRzyzVector::buildFrom(const double phi, const double theta, const double psi) { data[0] = phi; data[1] = theta; @@ -210,10 +196,8 @@ int main() vpRzyzVector &vpRzyzVector::operator=(const vpColVector &rzyz) { if (rzyz.size() != 3) { - throw(vpException( - vpException::dimensionError, - "Cannot set a R-zyz vector from a %d-dimension col vector", - rzyz.size())); + throw(vpException(vpException::dimensionError, "Cannot set a R-zyz vector from a %d-dimension col vector", + rzyz.size())); } for (unsigned int i = 0; i < 3; i++) data[i] = rzyz[i]; diff --git a/modules/core/src/math/transformation/vpThetaUVector.cpp b/modules/core/src/math/transformation/vpThetaUVector.cpp index c769ea2015..14dd03ae07 100644 --- a/modules/core/src/math/transformation/vpThetaUVector.cpp +++ b/modules/core/src/math/transformation/vpThetaUVector.cpp @@ -52,18 +52,13 @@ 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) -{ -} +vpThetaUVector::vpThetaUVector(const vpThetaUVector &tu) : vpRotationVector(tu) {} /*! Copy constructor from a 3-dimension vector. */ vpThetaUVector::vpThetaUVector(const vpColVector &tu) : vpRotationVector(3) { if (tu.size() != 3) { - throw(vpException( - vpException::dimensionError, - "Cannot construct a theta-u vector from a %d-dimension col vector", - tu.size())); + throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension col vector", + tu.size())); } for (unsigned int i = 0; i < 3; i++) data[i] = tu[i]; @@ -72,67 +67,41 @@ vpThetaUVector::vpThetaUVector(const vpColVector &tu) : vpRotationVector(3) /*! Initialize a \f$\theta {\bf u}\f$ vector from an homogeneous matrix. */ -vpThetaUVector::vpThetaUVector(const vpHomogeneousMatrix &M) - : vpRotationVector(3) -{ - buildFrom(M); -} +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); -} +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); -} +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); -} +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); -} +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); -} +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); -} +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) +vpThetaUVector::vpThetaUVector(const double tux, const double tuy, const double tuz) : vpRotationVector(3) { buildFrom(tux, tuy, tuz); } @@ -168,8 +137,7 @@ vpThetaUVector vpThetaUVector::buildFrom(const vpRotationMatrix &R) { double s, c, theta; - 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]) + + 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; @@ -312,10 +280,8 @@ int main() vpThetaUVector &vpThetaUVector::operator=(const vpColVector &tu) { if (tu.size() != 3) { - throw(vpException( - vpException::dimensionError, - "Cannot set a theta-u vector from a %d-dimension col vector", - tu.size())); + throw(vpException(vpException::dimensionError, "Cannot set a theta-u vector from a %d-dimension col vector", + tu.size())); } for (unsigned int i = 0; i < 3; i++) data[i] = tu[i]; @@ -339,8 +305,7 @@ vpThetaUVector &vpThetaUVector::operator=(const vpColVector &tu) int main() { - vpHomogeneousMatrix M(0, 0, 1., vpMath::rad(10), vpMath::rad(20), -vpMath::rad(30)); + vpHomogeneousMatrix M(0, 0, 1., vpMath::rad(10), vpMath::rad(20), vpMath::rad(30)); double theta; vpColVector u; @@ -379,21 +344,16 @@ representation. int main() { - vpHomogeneousMatrix M(0, 0, 1., vpMath::rad(10), vpMath::rad(20), -vpMath::rad(30)); + vpHomogeneousMatrix M(0, 0, 1., vpMath::rad(10), vpMath::rad(20), vpMath::rad(30)); - std::cout << "theta: " << M.getRotationMatrix().getThetaUVector().getTheta() -<< std::endl; std::cout << "u : " << -M.getRotationMatrix().getThetaUVector().getU().t() << std::endl; + std::cout << "theta: " << M.getRotationMatrix().getThetaUVector().getTheta() << std::endl; + std::cout << "u : " << M.getRotationMatrix().getThetaUVector().getU().t() << std::endl; } \endcode \sa getTheta(), extract() */ -double vpThetaUVector::getTheta() const -{ - return sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]); -} +double vpThetaUVector::getTheta() const { return sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]); } /*! @@ -409,12 +369,10 @@ representation. int main() { - vpHomogeneousMatrix M(0, 0, 1., vpMath::rad(10), vpMath::rad(20), -vpMath::rad(30)); + vpHomogeneousMatrix M(0, 0, 1., vpMath::rad(10), vpMath::rad(20), pMath::rad(30)); - std::cout << "theta: " << M.getRotationMatrix().getThetaUVector().getTheta() -<< std::endl; std::cout << "u : " << -M.getRotationMatrix().getThetaUVector().getU().t() << std::endl; + std::cout << "theta: " << M.getRotationMatrix().getThetaUVector().getTheta() << std::endl; + std::cout << "u : " << M.getRotationMatrix().getThetaUVector().getU().t() << std::endl; } \endcode @@ -438,8 +396,7 @@ vpColVector vpThetaUVector::getU() const /*! 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) +void vpThetaUVector::buildFrom(const double tux, const double tuy, const double tuz) { data[0] = tux; data[1] = tuy; diff --git a/modules/core/src/math/transformation/vpTranslationVector.cpp b/modules/core/src/math/transformation/vpTranslationVector.cpp index 44d5c5aed4..321513321a 100644 --- a/modules/core/src/math/transformation/vpTranslationVector.cpp +++ b/modules/core/src/math/transformation/vpTranslationVector.cpp @@ -54,9 +54,7 @@ in meters. */ -vpTranslationVector::vpTranslationVector(const double tx, const double ty, - const double tz) - : vpArray2D(3, 1) +vpTranslationVector::vpTranslationVector(const double tx, const double ty, const double tz) : vpArray2D(3, 1) { (*this)[0] = tx; (*this)[1] = ty; @@ -70,11 +68,7 @@ vpTranslationVector::vpTranslationVector(const double tx, const double ty, \param M : Homogeneous matrix where translations are in meters. */ -vpTranslationVector::vpTranslationVector(const vpHomogeneousMatrix &M) - : vpArray2D(3, 1) -{ - M.extract(*this); -} +vpTranslationVector::vpTranslationVector(const vpHomogeneousMatrix &M) : vpArray2D(3, 1) { M.extract(*this); } /*! Construct a translation vector \f$ \bf t \f$ from the translation contained @@ -83,8 +77,7 @@ vpTranslationVector::vpTranslationVector(const vpHomogeneousMatrix &M) \param p : Pose vector where translations are in meters. */ -vpTranslationVector::vpTranslationVector(const vpPoseVector &p) - : vpArray2D(3, 1) +vpTranslationVector::vpTranslationVector(const vpPoseVector &p) : vpArray2D(3, 1) { (*this)[0] = p[0]; (*this)[1] = p[1]; @@ -101,10 +94,7 @@ vpTranslationVector::vpTranslationVector(const vpPoseVector &p) vpTranslationVector t2(t1); // t2 is now a copy of t1 \endcode */ -vpTranslationVector::vpTranslationVector(const vpTranslationVector &tv) - : vpArray2D(tv) -{ -} +vpTranslationVector::vpTranslationVector(const vpTranslationVector &tv) : vpArray2D(tv) {} /*! Construct a translation vector \f$ \bf t \f$ from a 3-dimension column @@ -120,8 +110,7 @@ vpTranslationVector::vpTranslationVector(const vpTranslationVector &tv) \endcode */ -vpTranslationVector::vpTranslationVector(const vpColVector &v) - : vpArray2D(v) +vpTranslationVector::vpTranslationVector(const vpColVector &v) : vpArray2D(v) { if (v.size() != 3) { throw(vpException(vpException::dimensionError, @@ -142,8 +131,7 @@ vpTranslationVector::vpTranslationVector(const vpColVector &v) \return The build translation vector. */ -vpTranslationVector -vpTranslationVector::buildFrom(const vpHomogeneousMatrix &M) +vpTranslationVector vpTranslationVector::buildFrom(const vpHomogeneousMatrix &M) { M.extract(*this); return *this; @@ -178,10 +166,8 @@ vpTranslationVector vpTranslationVector::buildFrom(const vpPoseVector &p) vpTranslationVector vpTranslationVector::buildFrom(const vpColVector &v) { if (v.size() != 3) { - throw(vpException( - vpException::dimensionError, - "Cannot build a translation vector from a %d-dimension column vector", - v.size())); + throw(vpException(vpException::dimensionError, + "Cannot build a translation vector from a %d-dimension column vector", v.size())); } (*this)[0] = v[0]; @@ -198,9 +184,7 @@ vpTranslationVector vpTranslationVector::buildFrom(const vpColVector &v) \return The build translation vector. \sa set() */ -vpTranslationVector vpTranslationVector::buildFrom(const double tx, - const double ty, - const double tz) +vpTranslationVector vpTranslationVector::buildFrom(const double tx, const double ty, const double tz) { set(tx, ty, tz); return *this; @@ -212,8 +196,7 @@ vpTranslationVector vpTranslationVector::buildFrom(const double tx, \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) +void vpTranslationVector::set(const double tx, const double ty, const double tz) { (*this)[0] = tx; (*this)[1] = ty; @@ -226,7 +209,10 @@ void vpTranslationVector::set(const double tx, const double ty, \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); + add. +\code + vpTranslationVector t1(1,2,3); + vpTranslationVector t2(4,5,6); vpTranslationVector t3; t3 = t2 + t1; @@ -235,8 +221,7 @@ void vpTranslationVector::set(const double tx, const double ty, \endcode */ -vpTranslationVector vpTranslationVector:: -operator+(const vpTranslationVector &tv) const +vpTranslationVector vpTranslationVector::operator+(const vpTranslationVector &tv) const { vpTranslationVector s; @@ -251,8 +236,14 @@ operator+(const vpTranslationVector &tv) const \param v : 3-dimension column vector to add. \return The sum of the current translation vector (*this) and the column - vector to add. \code vpTranslationVector t1(1,2,3); vpColVector v(3); v[0] = - 4; v[1] = 5; v[2] = 6; vpTranslationVector t2; + vector to add. +\code + vpTranslationVector t1(1,2,3); + vpColVector v(3); + v[0] = 4; + v[1] = 5; + v[2] = 6; + vpTranslationVector t2; t2 = t1 + v; // t1 and v leave unchanged @@ -263,10 +254,8 @@ operator+(const vpTranslationVector &tv) const vpTranslationVector vpTranslationVector::operator+(const vpColVector &v) const { if (v.size() != 3) { - throw(vpException( - vpException::dimensionError, - "Cannot add translation vector to a %d-dimension column vector", - v.size())); + throw(vpException(vpException::dimensionError, "Cannot add translation vector to a %d-dimension column vector", + v.size())); } vpTranslationVector s; @@ -282,8 +271,11 @@ vpTranslationVector vpTranslationVector::operator+(const vpColVector &v) const \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; + one to substract. + \code + vpTranslationVector t1(1,2,3); + vpTranslationVector t2(4,5,6); + vpTranslationVector t3; t3 = t2 - t1; // t1 and t2 leave unchanged @@ -291,8 +283,7 @@ vpTranslationVector vpTranslationVector::operator+(const vpColVector &v) const \endcode */ -vpTranslationVector vpTranslationVector:: -operator-(const vpTranslationVector &tv) const +vpTranslationVector vpTranslationVector::operator-(const vpTranslationVector &tv) const { vpTranslationVector sub; @@ -477,8 +468,7 @@ vpTranslationVector &vpTranslationVector::operator=(const vpColVector &tv) // t2 is now equal to t1 : 1, 2, 3 \endcode */ -vpTranslationVector &vpTranslationVector:: -operator=(const vpTranslationVector &tv) +vpTranslationVector &vpTranslationVector::operator=(const vpTranslationVector &tv) { unsigned int k = tv.rowNum; if (rowNum != k) { @@ -605,8 +595,7 @@ vpMatrix vpTranslationVector::skew() const \return The cross product of two translation vectors \f$a \times b\f$. */ -vpTranslationVector vpTranslationVector::cross(const vpTranslationVector &a, - const vpTranslationVector &b) +vpTranslationVector vpTranslationVector::cross(const vpTranslationVector &a, const vpTranslationVector &b) { vpMatrix skew_a = vpTranslationVector::skew(a); return (vpTranslationVector)(skew_a * b); diff --git a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp index 2542ae1b86..6bc7fb097d 100644 --- a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp +++ b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp @@ -55,8 +55,7 @@ \param V : Velocity twist matrix to copy. */ -vpVelocityTwistMatrix &vpVelocityTwistMatrix:: -operator=(const vpVelocityTwistMatrix &V) +vpVelocityTwistMatrix &vpVelocityTwistMatrix::operator=(const vpVelocityTwistMatrix &V) { for (int i = 0; i < 6; i++) { for (int j = 0; j < 6; j++) { @@ -83,10 +82,7 @@ void vpVelocityTwistMatrix::eye() /*! Initialize a velocity twist transformation matrix as identity. */ -vpVelocityTwistMatrix::vpVelocityTwistMatrix() : vpArray2D(6, 6) -{ - eye(); -} +vpVelocityTwistMatrix::vpVelocityTwistMatrix() : vpArray2D(6, 6) { eye(); } /*! Initialize a velocity twist transformation matrix from another velocity @@ -94,11 +90,7 @@ vpVelocityTwistMatrix::vpVelocityTwistMatrix() : vpArray2D(6, 6) \param V : Velocity twist matrix used as initializer. */ -vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V) - : vpArray2D(6, 6) -{ - *this = V; -} +vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V) : vpArray2D(6, 6) { *this = V; } /*! @@ -118,9 +110,7 @@ vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V) {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f] */ -vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpHomogeneousMatrix &M, - bool full) - : vpArray2D(6, 6) +vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpHomogeneousMatrix &M, bool full) : vpArray2D(6, 6) { if (full) buildFrom(M); @@ -142,8 +132,7 @@ vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpHomogeneousMatrix &M, vector \f$R\f$ . */ -vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t, - const vpThetaUVector &thetau) +vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t, const vpThetaUVector &thetau) : vpArray2D(6, 6) { buildFrom(t, thetau); @@ -161,8 +150,7 @@ vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t, vector \f$R\f$ . */ -vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpThetaUVector &thetau) - : vpArray2D(6, 6) +vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpThetaUVector &thetau) : vpArray2D(6, 6) { buildFrom(thetau); } @@ -180,8 +168,7 @@ vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpThetaUVector &thetau) \param R : Rotation matrix. */ -vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t, - const vpRotationMatrix &R) +vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t, const vpRotationMatrix &R) : vpArray2D(6, 6) { buildFrom(t, R); @@ -198,11 +185,7 @@ vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t, \param R : Rotation matrix. */ -vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpRotationMatrix &R) - : vpArray2D(6, 6) -{ - buildFrom(R); -} +vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpRotationMatrix &R) : vpArray2D(6, 6) { buildFrom(R); } /*! @@ -219,11 +202,8 @@ vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpRotationMatrix &R) \param tux,tuy,tuz : \f$\theta {\bf u}\f$ rotation vector expressed in radians used to initialize \f$R\f$. */ -vpVelocityTwistMatrix::vpVelocityTwistMatrix(const double tx, const double ty, - const double tz, - const double tux, - const double tuy, - const double tuz) +vpVelocityTwistMatrix::vpVelocityTwistMatrix(const double tx, const double ty, const double tz, const double tux, + const double tuy, const double tuz) : vpArray2D(6, 6) { vpTranslationVector t(tx, ty, tz); @@ -237,8 +217,7 @@ vpVelocityTwistMatrix::vpVelocityTwistMatrix(const double tx, const double ty, an other velocity twist transformation matrix. */ -vpVelocityTwistMatrix vpVelocityTwistMatrix:: -operator*(const vpVelocityTwistMatrix &V) const +vpVelocityTwistMatrix vpVelocityTwistMatrix::operator*(const vpVelocityTwistMatrix &V) const { vpVelocityTwistMatrix p; @@ -276,8 +255,8 @@ int main() 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); + vpVelocityTwistMatrix cVe; // Velocity skew transformation from camera frame to end-effector + robot.get_cVe(cVe); vpMatrix eJe; // Robot jacobian robot.get_eJe(eJe); @@ -295,10 +274,8 @@ 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())); + 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()); @@ -359,8 +336,7 @@ vpColVector vpVelocityTwistMatrix::operator*(const vpColVector &v) const \param R : Rotation matrix. */ -vpVelocityTwistMatrix -vpVelocityTwistMatrix::buildFrom(const vpRotationMatrix &R) +vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpRotationMatrix &R) { for (unsigned int i = 0; i < 3; i++) { for (unsigned int j = 0; j < 3; j++) { @@ -386,9 +362,7 @@ vpVelocityTwistMatrix::buildFrom(const vpRotationMatrix &R) \param R : Rotation matrix. */ -vpVelocityTwistMatrix -vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t, - const vpRotationMatrix &R) +vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R) { vpMatrix skewaR = t.skew(t) * R; @@ -418,9 +392,7 @@ vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t, matrix \f${\bf R}\f$. */ -vpVelocityTwistMatrix -vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t, - const vpThetaUVector &thetau) +vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t, const vpThetaUVector &thetau) { buildFrom(t, vpRotationMatrix(thetau)); return (*this); @@ -438,8 +410,7 @@ vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t, matrix \f${\bf R}\f$. */ -vpVelocityTwistMatrix -vpVelocityTwistMatrix::buildFrom(const vpThetaUVector &thetau) +vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpThetaUVector &thetau) { buildFrom(vpRotationMatrix(thetau)); return (*this); @@ -463,8 +434,7 @@ vpVelocityTwistMatrix::buildFrom(const vpThetaUVector &thetau) {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f] */ -vpVelocityTwistMatrix -vpVelocityTwistMatrix::buildFrom(const vpHomogeneousMatrix &M, bool full) +vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpHomogeneousMatrix &M, bool full) { if (full) buildFrom(M.getTranslationVector(), M.getRotationMatrix()); @@ -491,10 +461,7 @@ vpVelocityTwistMatrix vpVelocityTwistMatrix::inverse() const } //! Invert the velocity twist matrix. -void vpVelocityTwistMatrix::inverse(vpVelocityTwistMatrix &V) const -{ - V = inverse(); -} +void vpVelocityTwistMatrix::inverse(vpVelocityTwistMatrix &V) const { V = inverse(); } //! Extract the rotation matrix from the velocity twist matrix. void vpVelocityTwistMatrix::extract(vpRotationMatrix &R) const @@ -539,8 +506,7 @@ void vpVelocityTwistMatrix::extract(vpTranslationVector &tv) const \sa std::ostream &operator<<(std::ostream &s, const vpArray2D &A) */ -int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, - char const *intro) const +int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, char const *intro) const { typedef std::string::size_type size_type; diff --git a/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp index c31f2d3189..930da906f3 100644 --- a/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp +++ b/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp @@ -74,16 +74,12 @@ /*! Default constructor */ -vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix() - : vpXmlParser(), m_M(), m_name() -{ -} +vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix() : vpXmlParser(), m_M(), m_name() {} /*! Copy constructor \param twinParser : parser object to copy */ -vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix( - vpXmlParserHomogeneousMatrix &twinParser) +vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix(vpXmlParserHomogeneousMatrix &twinParser) : vpXmlParser(twinParser), m_M(), m_name() { *this = twinParser; @@ -94,8 +90,7 @@ vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix( \param twinParser : parser object to copy \return a copy of the input. */ -vpXmlParserHomogeneousMatrix &vpXmlParserHomogeneousMatrix:: -operator=(const vpXmlParserHomogeneousMatrix &twinParser) +vpXmlParserHomogeneousMatrix &vpXmlParserHomogeneousMatrix::operator=(const vpXmlParserHomogeneousMatrix &twinParser) { this->m_M = twinParser.m_M; this->m_name = twinParser.m_name; @@ -111,9 +106,7 @@ operator=(const vpXmlParserHomogeneousMatrix &twinParser) \return error code. */ -int vpXmlParserHomogeneousMatrix::parse(vpHomogeneousMatrix &M, - const std::string &filename, - const std::string &name) +int vpXmlParserHomogeneousMatrix::parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name) { xmlDocPtr doc; xmlNodePtr node; @@ -149,26 +142,22 @@ int vpXmlParserHomogeneousMatrix::parse(vpHomogeneousMatrix &M, \return error code. */ -int vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, - const std::string &filename, +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); + 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 https://visp.inria.fr/download/ .\n" - " It can be read with the parse method of\n" - " the vpXmlParserHomogeneousMatrix class."); + xmlNodePtr node_tmp = xmlNewComment((xmlChar *)"This file stores homogeneous matrix used\n" + " in the vpHomogeneousMatrix Class of ViSP available\n" + " at https://visp.inria.fr/download/ .\n" + " It can be read with the parse method of\n" + " the vpXmlParserHomogeneousMatrix class."); xmlAddChild(node, node_tmp); } @@ -185,8 +174,7 @@ int vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, 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 + << "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; @@ -210,8 +198,7 @@ int vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, \param name : name of the Homogeneous Matrix \return error code. */ -int vpXmlParserHomogeneousMatrix::read(xmlDocPtr doc, xmlNodePtr node, - const std::string &name) +int vpXmlParserHomogeneousMatrix::read(xmlDocPtr doc, xmlNodePtr node, const std::string &name) { // char * val_char; vpXmlCodeType prop; @@ -236,8 +223,7 @@ int vpXmlParserHomogeneousMatrix::read(xmlDocPtr doc, xmlNodePtr node, if (nbM == 0) { back = SEQUENCE_ERROR; - vpCERROR << "No Homogeneous matrix is available" << std::endl - << "with name: " << name << std::endl; + 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 @@ -258,8 +244,7 @@ int vpXmlParserHomogeneousMatrix::read(xmlDocPtr doc, xmlNodePtr node, \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) +int vpXmlParserHomogeneousMatrix::count(xmlDocPtr doc, xmlNodePtr node, const std::string &name) { // char * val_char; vpXmlCodeType prop; @@ -290,8 +275,7 @@ int vpXmlParserHomogeneousMatrix::count(xmlDocPtr doc, xmlNodePtr node, \return error code. */ -int vpXmlParserHomogeneousMatrix::read_matrix(xmlDocPtr doc, xmlNodePtr node, - const std::string &name) +int vpXmlParserHomogeneousMatrix::read_matrix(xmlDocPtr doc, xmlNodePtr node, const std::string &name) { vpXmlCodeType prop; /* read value in the XML file. */ @@ -319,8 +303,7 @@ int vpXmlParserHomogeneousMatrix::read_matrix(xmlDocPtr doc, xmlNodePtr node, case CODE_XML_VALUE: // VALUE if (name == M_name_tmp) { - std::cout << "Found Homogeneous Matrix with name: \"" << M_name_tmp - << "\"" << std::endl; + std::cout << "Found Homogeneous Matrix with name: \"" << M_name_tmp << "\"" << std::endl; back = read_values(doc, node, M_tmp); } break; @@ -363,8 +346,7 @@ int vpXmlParserHomogeneousMatrix::read_matrix(xmlDocPtr doc, xmlNodePtr node, */ vpXmlParserHomogeneousMatrix::vpXmlCodeSequenceType -vpXmlParserHomogeneousMatrix::read_values(xmlDocPtr doc, xmlNodePtr node, - vpHomogeneousMatrix &M) +vpXmlParserHomogeneousMatrix::read_values(xmlDocPtr doc, xmlNodePtr node, vpHomogeneousMatrix &M) { // counter of the number of read parameters int nb = 0; @@ -459,8 +441,7 @@ vpXmlParserHomogeneousMatrix::read_values(xmlDocPtr doc, xmlNodePtr node, \return error code. */ -int vpXmlParserHomogeneousMatrix::write(xmlNodePtr node, - const std::string &name) +int vpXmlParserHomogeneousMatrix::write(xmlNodePtr node, const std::string &name) { int back = SEQUENCE_OK; @@ -485,8 +466,7 @@ int vpXmlParserHomogeneousMatrix::write(xmlNodePtr node, 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()); + xmlNewTextChild(node_matrix, NULL, (xmlChar *)LABEL_XML_M_NAME, (xmlChar *)name.c_str()); } // @@ -496,44 +476,36 @@ int vpXmlParserHomogeneousMatrix::write(xmlNodePtr node, { char str[11]; - node_tmp = xmlNewComment( - (xmlChar *)"Translation vector with values in meters"); + node_tmp = xmlNewComment((xmlChar *)"Translation vector with values in meters"); xmlAddChild(node_values, node_tmp); // sprintf(str, "%f", m_M[0][3]); - xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TX, - (xmlChar *)str); + xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TX, (xmlChar *)str); // sprintf(str, "%f", m_M[1][3]); - xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TY, - (xmlChar *)str); + xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TY, (xmlChar *)str); // sprintf(str, "%f", m_M[2][3]); - xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TZ, - (xmlChar *)str); + 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"); + node_tmp = xmlNewComment((xmlChar *)"Rotational vector expressed in angle axis " + "representation with values in radians"); xmlAddChild(node_values, node_tmp); // sprintf(str, "%f", tu[0]); - xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TUX, - (xmlChar *)str); + xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TUX, (xmlChar *)str); // sprintf(str, "%f", tu[1]); - xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TUY, - (xmlChar *)str); + xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TUY, (xmlChar *)str); // sprintf(str, "%f", tu[2]); - xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TUZ, - (xmlChar *)str); + xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TUZ, (xmlChar *)str); } } return back; @@ -547,8 +519,8 @@ int vpXmlParserHomogeneousMatrix::write(xmlNodePtr node, \return error code. */ -vpXmlParserHomogeneousMatrix::vpXmlCodeSequenceType -vpXmlParserHomogeneousMatrix::str2xmlcode(char *str, vpXmlCodeType &res) +vpXmlParserHomogeneousMatrix::vpXmlCodeSequenceType vpXmlParserHomogeneousMatrix::str2xmlcode(char *str, + vpXmlCodeType &res) { vpXmlCodeType val_int = CODE_XML_BAD; vpXmlCodeSequenceType back = vpXmlParserHomogeneousMatrix::SEQUENCE_OK; diff --git a/modules/core/src/tools/convert/vpConvert.cpp b/modules/core/src/tools/convert/vpConvert.cpp index 95f9f57d42..ae603351dd 100644 --- a/modules/core/src/tools/convert/vpConvert.cpp +++ b/modules/core/src/tools/convert/vpConvert.cpp @@ -66,10 +66,7 @@ vpImagePoint vpConvert::keyPointToVpImagePoint(const cv::KeyPoint &keypoint) \return A vpImagePoint with the 2D coordinates stored in cv::Point2f. */ -vpImagePoint vpConvert::point2fToVpImagePoint(const cv::Point2f &point) -{ - return vpImagePoint(point.y, point.x); -} +vpImagePoint vpConvert::point2fToVpImagePoint(const cv::Point2f &point) { return vpImagePoint(point.y, point.x); } /*! Unary function to convert a cv::Point2d to a vpImagePoint. @@ -77,10 +74,7 @@ vpImagePoint vpConvert::point2fToVpImagePoint(const cv::Point2f &point) \return A vpImagePoint with the 2D coordinates stored in cv::Point2d. */ -vpImagePoint vpConvert::point2dToVpImagePoint(const cv::Point2d &point) -{ - return vpImagePoint(point.y, point.x); -} +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). @@ -177,8 +171,7 @@ cv::Point2d vpConvert::vpImagePointToPoint2d(const vpImagePoint &point) */ cv::Point3f vpConvert::vpCamPointToPoint3f(const vpPoint &point) { - return cv::Point3f((float)point.get_X(), (float)point.get_Y(), - (float)point.get_Z()); + return cv::Point3f((float)point.get_X(), (float)point.get_Y(), (float)point.get_Z()); } /*! @@ -202,8 +195,7 @@ cv::Point3d vpConvert::vpCamPointToPoint3d(const vpPoint &point) */ cv::Point3f vpConvert::vpObjectPointToPoint3f(const vpPoint &point) { - return cv::Point3f((float)point.get_oX(), (float)point.get_oY(), - (float)point.get_oZ()); + return cv::Point3f((float)point.get_oX(), (float)point.get_oY(), (float)point.get_oZ()); } /*! @@ -224,40 +216,28 @@ cv::Point3d vpConvert::vpObjectPointToPoint3d(const vpPoint &point) \return The train index stored in a cv::DMatch. */ -int vpConvert::dMatchToTrainIndex(const cv::DMatch &match) -{ - return match.trainIdx; -} +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); -} +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); -} +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); -} +void vpConvert::convertFromOpenCV(const cv::Point2d &from, vpImagePoint &to) { to = point2dToVpImagePoint(from); } /*! Convert a cv::Point3f to a vpPoint. @@ -266,8 +246,7 @@ void vpConvert::convertFromOpenCV(const cv::Point2d &from, vpImagePoint &to) \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) +void vpConvert::convertFromOpenCV(const cv::Point3f &from, vpPoint &to, const bool cameraFrame) { if (cameraFrame) { to = point3fToVpCamPoint(from); @@ -283,8 +262,7 @@ void vpConvert::convertFromOpenCV(const cv::Point3f &from, vpPoint &to, \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) +void vpConvert::convertFromOpenCV(const cv::Point3d &from, vpPoint &to, const bool cameraFrame) { if (cameraFrame) { to = point3dToVpCamPoint(from); @@ -298,12 +276,10 @@ void vpConvert::convertFromOpenCV(const cv::Point3d &from, vpPoint &to, \param from : Vector of cv::KeyPoint to convert. \param to : Vector of vpImagePoint converted. */ -void vpConvert::convertFromOpenCV(const std::vector &from, - std::vector &to) +void vpConvert::convertFromOpenCV(const std::vector &from, std::vector &to) { to.resize(from.size()); - std::transform(from.begin(), from.end(), to.begin(), - keyPointToVpImagePoint); + std::transform(from.begin(), from.end(), to.begin(), keyPointToVpImagePoint); } /*! @@ -311,8 +287,7 @@ void vpConvert::convertFromOpenCV(const std::vector &from, \param from : Vector of cv::Point2f to convert. \param to : Vector of vpImagePoint converted. */ -void vpConvert::convertFromOpenCV(const std::vector &from, - std::vector &to) +void vpConvert::convertFromOpenCV(const std::vector &from, std::vector &to) { to.resize(from.size()); std::transform(from.begin(), from.end(), to.begin(), point2fToVpImagePoint); @@ -323,8 +298,7 @@ void vpConvert::convertFromOpenCV(const std::vector &from, \param from : Vector of cv::Point2d to convert. \param to : Vector of vpImagePoint converted. */ -void vpConvert::convertFromOpenCV(const std::vector &from, - std::vector &to) +void vpConvert::convertFromOpenCV(const std::vector &from, std::vector &to) { to.resize(from.size()); std::transform(from.begin(), from.end(), to.begin(), point2dToVpImagePoint); @@ -337,16 +311,14 @@ void vpConvert::convertFromOpenCV(const std::vector &from, \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame. */ -void vpConvert::convertFromOpenCV(const std::vector &from, - std::vector &to, +void vpConvert::convertFromOpenCV(const std::vector &from, std::vector &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); + std::transform(from.begin(), from.end(), to.begin(), point3fToVpObjectPoint); } } @@ -357,16 +329,14 @@ void vpConvert::convertFromOpenCV(const std::vector &from, \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame. */ -void vpConvert::convertFromOpenCV(const std::vector &from, - std::vector &to, +void vpConvert::convertFromOpenCV(const std::vector &from, std::vector &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); + std::transform(from.begin(), from.end(), to.begin(), point3dToVpObjectPoint); } } @@ -380,8 +350,7 @@ void vpConvert::convertFromOpenCV(const std::vector &from, \param from : Vector of cv::DMatch to convert. \param to : Vector of unsigned int converted. */ -void vpConvert::convertFromOpenCV(const std::vector &from, - std::vector &to) +void vpConvert::convertFromOpenCV(const std::vector &from, std::vector &to) { to.resize(from.size()); std::transform(from.begin(), from.end(), to.begin(), dMatchToTrainIndex); @@ -392,20 +361,14 @@ void vpConvert::convertFromOpenCV(const std::vector &from, \param from : vpImagePoint to convert. \param to : cv::Point2f converted. */ -void vpConvert::convertToOpenCV(const vpImagePoint &from, cv::Point2f &to) -{ - to = vpImagePointToPoint2f(from); -} +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); -} +void vpConvert::convertToOpenCV(const vpImagePoint &from, cv::Point2d &to) { to = vpImagePointToPoint2d(from); } /*! Convert a vpPoint to a cv::Point3f. @@ -414,8 +377,7 @@ void vpConvert::convertToOpenCV(const vpImagePoint &from, cv::Point2d &to) \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) +void vpConvert::convertToOpenCV(const vpPoint &from, cv::Point3f &to, const bool cameraFrame) { if (cameraFrame) { to = vpCamPointToPoint3f(from); @@ -431,8 +393,7 @@ void vpConvert::convertToOpenCV(const vpPoint &from, cv::Point3f &to, \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) +void vpConvert::convertToOpenCV(const vpPoint &from, cv::Point3d &to, const bool cameraFrame) { if (cameraFrame) { to = vpCamPointToPoint3d(from); @@ -446,8 +407,7 @@ void vpConvert::convertToOpenCV(const vpPoint &from, cv::Point3d &to, \param from : Vector of vpImagePoint to convert. \param to : Vector of cv::Point2f converted. */ -void vpConvert::convertToOpenCV(const std::vector &from, - std::vector &to) +void vpConvert::convertToOpenCV(const std::vector &from, std::vector &to) { to.resize(from.size()); std::transform(from.begin(), from.end(), to.begin(), vpImagePointToPoint2f); @@ -458,8 +418,7 @@ void vpConvert::convertToOpenCV(const std::vector &from, \param from : Vector of vpImagePoint to convert. \param to : Vector of cv::Point2d converted. */ -void vpConvert::convertToOpenCV(const std::vector &from, - std::vector &to) +void vpConvert::convertToOpenCV(const std::vector &from, std::vector &to) { to.resize(from.size()); std::transform(from.begin(), from.end(), to.begin(), vpImagePointToPoint2d); @@ -472,16 +431,13 @@ void vpConvert::convertToOpenCV(const std::vector &from, \param cameraFrame : If true, the camera frame is considered, otherwise the object frame. */ -void vpConvert::convertToOpenCV(const std::vector &from, - std::vector &to, - const bool cameraFrame) +void vpConvert::convertToOpenCV(const std::vector &from, std::vector &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); + std::transform(from.begin(), from.end(), to.begin(), vpObjectPointToPoint3f); } } @@ -492,16 +448,13 @@ void vpConvert::convertToOpenCV(const std::vector &from, \param cameraFrame : If true, the camera frame is considered, otherwise the object frame. */ -void vpConvert::convertToOpenCV(const std::vector &from, - std::vector &to, - const bool cameraFrame) +void vpConvert::convertToOpenCV(const std::vector &from, std::vector &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); + std::transform(from.begin(), from.end(), to.begin(), vpObjectPointToPoint3d); } } #elif !defined(VISP_BUILD_SHARED_LIBS) diff --git a/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp b/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp index bf48223d3a..4a5c04832f 100644 --- a/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp +++ b/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp @@ -21,8 +21,7 @@ //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// -#if defined(__x86_64__) || defined(_M_X64) || defined(__i386) || \ - defined(_M_IX86) +#if defined(__x86_64__) || defined(_M_X64) || defined(__i386) || defined(_M_IX86) #if _WIN32 #include "cpu_x86_Windows.ipp" #elif defined(__GNUC__) || defined(__clang__) diff --git a/modules/core/src/tools/exceptions/vpException.cpp b/modules/core/src/tools/exceptions/vpException.cpp index ddb2cdf894..c6821670c9 100644 --- a/modules/core/src/tools/exceptions/vpException.cpp +++ b/modules/core/src/tools/exceptions/vpException.cpp @@ -51,13 +51,9 @@ */ vpException::vpException(int id) : code(id), message() {} -vpException::vpException(int id, const std::string &msg) - : code(id), message(msg) -{ -} +vpException::vpException(int id, const std::string &msg) : code(id), message(msg) {} -vpException::vpException(int id, const char *format, ...) - : code(id), message() +vpException::vpException(int id, const char *format, ...) : code(id), message() { va_list args; va_start(args, format); @@ -65,8 +61,7 @@ vpException::vpException(int id, const char *format, ...) va_end(args); } -vpException::vpException(const int id, const char *format, va_list args) - : code(id), message() +vpException::vpException(const int id, const char *format, va_list args) : code(id), message() { setMessage(format, args); } @@ -92,15 +87,9 @@ void vpException::setMessage(const char *format, va_list args) /* --- ACCESSORS ---------------------------------------------------------- */ /* ------------------------------------------------------------------------ */ -const char *vpException::getMessage(void) const -{ - return (this->message).c_str(); -} +const char *vpException::getMessage(void) const { return (this->message).c_str(); } -const std::string &vpException::getStringMessage(void) const -{ - return this->message; -} +const std::string &vpException::getStringMessage(void) const { return this->message; } int vpException::getCode(void) { return this->code; } @@ -110,10 +99,7 @@ int vpException::getCode(void) { return this->code; } \return pointer on the array of \e char related to the error string. */ -const char *vpException::what() const throw() -{ - return (this->message).c_str(); -} +const char *vpException::what() const throw() { return (this->message).c_str(); } /* ------------------------------------------------------------------------- */ @@ -129,8 +115,7 @@ const char *vpException::what() const throw() /* ------------------------------------------------------------------------- */ -VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpException &error) +VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpException &error) { os << "Error [" << error.code << "]:\t" << error.message << std::endl; diff --git a/modules/core/src/tools/file/vpIoTools.cpp b/modules/core/src/tools/file/vpIoTools.cpp index a7786c0df2..0b27364580 100644 --- a/modules/core/src/tools/file/vpIoTools.cpp +++ b/modules/core/src/tools/file/vpIoTools.cpp @@ -54,8 +54,7 @@ #include #include #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #include #include #elif defined(_WIN32) @@ -67,7 +66,7 @@ #endif #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin) -#include // To detect OSX or IOS using TARGET_OS_IOS macro +#include // To detect OSX or IOS using TARGET_OS_IOS macro #endif #ifndef PATH_MAX @@ -84,8 +83,7 @@ std::vector vpIoTools::configValues = std::vector(); // wordexp() is not available namespace { -void replaceAll(std::string &str, const std::string &search, - const std::string &replace) +void replaceAll(std::string &str, const std::string &search, const std::string &replace) { size_t start_pos = 0; while ((start_pos = str.find(search, start_pos)) != std::string::npos) { @@ -153,16 +151,13 @@ std::string vpIoTools::getFullName() { return baseDir + baseName; } 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 +#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")); + vpERROR_TRACE("Cannot get the username. Check your LOGNAME environment variable"); + throw(vpIoException(vpIoException::cantGetUserName, "Cannot get the username")); } username = _username; #elif defined(_WIN32) @@ -173,15 +168,13 @@ void vpIoTools::getUserName(std::string &username) // Get the user name. if (!GetUserName(infoBuf, &bufCharCount)) { delete[] infoBuf; - throw(vpIoException(vpIoException::cantGetUserName, - "Cannot get the username")); + throw(vpIoException(vpIoException::cantGetUserName, "Cannot get the username")); } username = infoBuf; delete[] infoBuf; #else - throw(vpIoException(vpIoException::cantGetUserName, - "Cannot get the username: not implemented on Universal " - "Windows Platform")); + throw(vpIoException(vpIoException::cantGetUserName, "Cannot get the username: not implemented on Universal " + "Windows Platform")); #endif #endif } @@ -205,16 +198,13 @@ void vpIoTools::getUserName(std::string &username) std::string vpIoTools::getUserName() { std::string username; -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#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")); + vpERROR_TRACE("Cannot get the username. Check your LOGNAME environment variable"); + throw(vpIoException(vpIoException::cantGetUserName, "Cannot get the username")); } username = _username; #elif defined(_WIN32) @@ -226,15 +216,13 @@ std::string vpIoTools::getUserName() if (!GetUserName(infoBuf, &bufCharCount)) { delete[] infoBuf; vpERROR_TRACE("Cannot get the username"); - throw(vpIoException(vpIoException::cantGetUserName, - "Cannot get the username")); + throw(vpIoException(vpIoException::cantGetUserName, "Cannot get the username")); } username = infoBuf; delete[] infoBuf; #else - throw(vpIoException(vpIoException::cantGetUserName, - "Cannot get the username: not implemented on Universal " - "Windows Platform")); + throw(vpIoException(vpIoException::cantGetUserName, "Cannot get the username: not implemented on Universal " + "Windows Platform")); #endif #endif return username; @@ -274,17 +262,15 @@ int main() std::string vpIoTools::getenv(const char *env) { #if defined(_WIN32) && defined(WINRT) - throw(vpIoException(vpIoException::cantGetenv, - "Cannot get the environment variable value: not " - "implemented on Universal Windows Platform")); + throw(vpIoException(vpIoException::cantGetenv, "Cannot get the environment variable value: not " + "implemented on Universal Windows Platform")); #else std::string value; // Get the environment variable value. char *_value = NULL; _value = ::getenv(env); if (_value == NULL) { - throw(vpIoException(vpIoException::cantGetenv, - "Cannot get the environment variable value")); + throw(vpIoException(vpIoException::cantGetenv, "Cannot get the environment variable value")); } value = _value; @@ -324,10 +310,7 @@ int main() \sa getenv(const char *) */ -std::string vpIoTools::getenv(const std::string &env) -{ - return (vpIoTools::getenv(env.c_str())); -} +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". @@ -338,8 +321,7 @@ std::string vpIoTools::getenv(const std::string &env) \param minor : Extracted minor. \param patch : Extracted patch. */ -void vpIoTools::getVersion(const std::string &version, unsigned int &major, - unsigned int &minor, unsigned int &patch) +void vpIoTools::getVersion(const std::string &version, unsigned int &major, unsigned int &minor, unsigned int &patch) { if (version.size() == 0) { major = 0; @@ -352,8 +334,7 @@ void vpIoTools::getVersion(const std::string &version, unsigned int &major, 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))); + 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) { @@ -385,8 +366,7 @@ void vpIoTools::getVersion(const std::string &version, unsigned int &major, */ bool vpIoTools::checkDirectory(const char *dirname) { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX struct stat stbuf; #elif defined(_WIN32) && defined(__MINGW32__) struct stat stbuf; @@ -400,15 +380,13 @@ bool vpIoTools::checkDirectory(const char *dirname) std::string _dirname = path(dirname); -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX if (stat(_dirname.c_str(), &stbuf) != 0) #elif defined(_WIN32) && defined(__MINGW32__) // Remove trailing separator character if any // AppVeyor: Windows 6.3.9600 AMD64 ; C:/MinGW/bin/g++.exe (ver 5.3.0) ; // GNU Make 3.82.90 Built for i686-pc-mingw32 - if (!_dirname.empty() && - _dirname.at(_dirname.size() - 1) == vpIoTools::separator) + if (!_dirname.empty() && _dirname.at(_dirname.size() - 1) == vpIoTools::separator) _dirname = _dirname.substr(0, _dirname.size() - 1); if (stat(_dirname.c_str(), &stbuf) != 0) #elif defined(_WIN32) @@ -417,15 +395,13 @@ bool vpIoTools::checkDirectory(const char *dirname) { return false; } -#if defined(_WIN32) || (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) +#if defined(_WIN32) || (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) if ((stbuf.st_mode & S_IFDIR) == 0) #endif { return false; } -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#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) @@ -449,10 +425,7 @@ bool vpIoTools::checkDirectory(const char *dirname) \sa checkDirectory(const char *) */ -bool vpIoTools::checkDirectory(const std::string &dirname) -{ - return vpIoTools::checkDirectory(dirname.c_str()); -} +bool vpIoTools::checkDirectory(const std::string &dirname) { return vpIoTools::checkDirectory(dirname.c_str()); } // See: // https://gist.github.com/JonathonReinhart/8c0d90191c38af2dcadb102c4e202950 @@ -478,8 +451,7 @@ int vpIoTools::mkdir_p(const char *path, const int mode) /* Temporarily truncate */ *p = '\0'; -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) if (mkdir(_path, (mode_t)mode) != 0) #elif defined(_WIN32) (void)mode; // var not used @@ -493,8 +465,7 @@ int vpIoTools::mkdir_p(const char *path, const int mode) } } -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) if (mkdir(_path, (mode_t)mode) != 0) #elif defined(_WIN32) if (_mkdir(_path) != 0) @@ -523,16 +494,12 @@ int vpIoTools::mkdir_p(const char *path, const int mode) */ void vpIoTools::makeDirectory(const char *dirname) { -#if ((!defined(__unix__) && !defined(__unix) && \ - (!defined(__APPLE__) || !defined(__MACH__)))) && \ - !defined(_WIN32) - std::cerr << "Unsupported platform for vpIoTools::makeDirectory()!" - << std::endl; +#if ((!defined(__unix__) && !defined(__unix) && (!defined(__APPLE__) || !defined(__MACH__)))) && !defined(_WIN32) + std::cerr << "Unsupported platform for vpIoTools::makeDirectory()!" << std::endl; return; #endif -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX struct stat stbuf; #elif defined(_WIN32) && defined(__MINGW32__) struct stat stbuf; @@ -542,14 +509,12 @@ void vpIoTools::makeDirectory(const char *dirname) if (dirname == NULL || dirname[0] == '\0') { vpERROR_TRACE("invalid directory name\n"); - throw(vpIoException(vpIoException::invalidDirectoryName, - "invalid directory name")); + 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 !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX if (stat(_dirname.c_str(), &stbuf) != 0) #elif defined(_WIN32) && defined(__MINGW32__) if (stat(_dirname.c_str(), &stbuf) != 0) @@ -559,8 +524,7 @@ void vpIoTools::makeDirectory(const char *dirname) { if (vpIoTools::mkdir_p(_dirname.c_str(), 0755) != 0) { vpERROR_TRACE("unable to create directory '%s'\n", dirname); - throw(vpIoException(vpIoException::cantCreateDirectory, - "unable to create directory")); + throw(vpIoException(vpIoException::cantCreateDirectory, "unable to create directory")); } vpDEBUG_TRACE(2, "has created directory '%s'\n", dirname); @@ -568,8 +532,7 @@ void vpIoTools::makeDirectory(const char *dirname) if (checkDirectory(dirname) == false) { vpERROR_TRACE("unable to create directory '%s'\n", dirname); - throw(vpIoException(vpIoException::cantCreateDirectory, - "unable to create directory")); + throw(vpIoException(vpIoException::cantCreateDirectory, "unable to create directory")); } } @@ -591,8 +554,7 @@ void vpIoTools::makeDirectory(const std::string &dirname) 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")); + throw(vpIoException(vpIoException::cantCreateDirectory, "unable to create directory")); } } @@ -610,8 +572,7 @@ void vpIoTools::makeDirectory(const std::string &dirname) */ bool vpIoTools::checkFilename(const char *filename) { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX struct stat stbuf; #elif defined(_WIN32) struct _stat stbuf; @@ -622,8 +583,7 @@ bool vpIoTools::checkFilename(const char *filename) } std::string _filename = path(filename); -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#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) @@ -634,8 +594,7 @@ bool vpIoTools::checkFilename(const char *filename) if ((stbuf.st_mode & S_IFREG) == 0) { return false; } -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#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) @@ -658,10 +617,7 @@ bool vpIoTools::checkFilename(const char *filename) \sa checkFilename(const char *) */ -bool vpIoTools::checkFilename(const std::string &filename) -{ - return vpIoTools::checkFilename(filename.c_str()); -} +bool vpIoTools::checkFilename(const std::string &filename) { return vpIoTools::checkFilename(filename.c_str()); } /*! @@ -680,8 +636,7 @@ bool vpIoTools::copy(const char *src, const char *dst) // 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 +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #if TARGET_OS_IOS == 0 // The following code is not working on iOS since // wordexp() is not available char cmd[FILENAME_MAX]; @@ -693,9 +648,7 @@ bool vpIoTools::copy(const char *src, const char *dst) // std::cout << cmd << " return value: " << ret << std::endl; return true; #else - throw(vpIoException( - vpException::fatalError, - "Cannot copy %s in %s: not implemented on iOS Platform", src, dst)); + throw(vpIoException(vpException::fatalError, "Cannot copy %s in %s: not implemented on iOS Platform", src, dst)); #endif #elif defined(_WIN32) #if (!defined(WINRT)) @@ -710,16 +663,13 @@ bool vpIoTools::copy(const char *src, const char *dst) // std::cout << cmd << " return value: " << ret << std::endl; return true; #else - throw(vpIoException( - vpException::fatalError, - "Cannot copy %s in %s: not implemented on Universal Windows Platform", - src, dst)); + throw(vpIoException(vpException::fatalError, "Cannot copy %s in %s: not implemented on Universal Windows Platform", + src, dst)); #endif #endif } 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 +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #if TARGET_OS_IOS == 0 // The following code is not working on iOS since // wordexp() is not available char cmd[FILENAME_MAX]; @@ -731,9 +681,7 @@ bool vpIoTools::copy(const char *src, const char *dst) // std::cout << cmd << " return value: " << ret << std::endl; return true; #else - throw(vpIoException( - vpException::fatalError, - "Cannot copy %s in %s: not implemented on iOS Platform", src, dst)); + throw(vpIoException(vpException::fatalError, "Cannot copy %s in %s: not implemented on iOS Platform", src, dst)); #endif #elif defined(_WIN32) #if (!defined(WINRT)) @@ -748,10 +696,8 @@ bool vpIoTools::copy(const char *src, const char *dst) // std::cout << cmd << " return value: " << ret << std::endl; return true; #else - throw(vpIoException( - vpException::fatalError, - "Cannot copy %s in %s: not implemented on Universal Windows Platform", - src, dst)); + throw(vpIoException(vpException::fatalError, "Cannot copy %s in %s: not implemented on Universal Windows Platform", + src, dst)); #endif #endif } else { @@ -797,8 +743,7 @@ bool vpIoTools::remove(const char *file_or_dir) return true; } else if (vpIoTools::checkDirectory(file_or_dir)) { // std::cout << "remove directory: " << file_or_dir << std::endl; -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #if TARGET_OS_IOS == 0 // The following code is not working on iOS since // wordexp() is not available char cmd[FILENAME_MAX]; @@ -809,9 +754,7 @@ bool vpIoTools::remove(const char *file_or_dir) // std::cout << cmd << " return value: " << ret << std::endl; return true; #else - throw(vpIoException(vpException::fatalError, - "Cannot remove %s: not implemented on iOS Platform", - file_or_dir)); + throw(vpIoException(vpException::fatalError, "Cannot remove %s: not implemented on iOS Platform", file_or_dir)); #endif #elif defined(_WIN32) #if (!defined(WINRT)) @@ -824,10 +767,8 @@ bool vpIoTools::remove(const char *file_or_dir) // std::cout << cmd << " return value: " << ret << std::endl; return true; #else - throw(vpIoException( - vpException::fatalError, - "Cannot remove %s: not implemented on Universal Windows Platform", - file_or_dir)); + throw(vpIoException(vpException::fatalError, "Cannot remove %s: not implemented on Universal Windows Platform", + file_or_dir)); #endif #endif } else { @@ -846,10 +787,7 @@ bool vpIoTools::remove(const char *file_or_dir) \sa remove(const char *) */ -bool vpIoTools::remove(const std::string &file_or_dir) -{ - return vpIoTools::remove(file_or_dir.c_str()); -} +bool vpIoTools::remove(const std::string &file_or_dir) { return vpIoTools::remove(file_or_dir.c_str()); } /*! @@ -881,8 +819,7 @@ bool vpIoTools::rename(const char *oldfilename, const char *newfilename) \sa rename(const char *, const char *) */ -bool vpIoTools::rename(const std::string &oldfilename, - const std::string &newfilename) +bool vpIoTools::rename(const std::string &oldfilename, const std::string &newfilename) { return vpIoTools::rename(oldfilename.c_str(), newfilename.c_str()); } @@ -907,8 +844,7 @@ std::string vpIoTools::path(const char *pathname) for (unsigned int i = 0; i < path.length(); i++) if (path[i] == '/') path[i] = '\\'; -#elif defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__)) +#elif defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)) for (unsigned int i = 0; i < path.length(); i++) if (path[i] == '\\') path[i] = '/'; @@ -940,10 +876,7 @@ std::string vpIoTools::path(const char *pathname) \sa path(const char *) */ -std::string vpIoTools::path(const std::string &pathname) -{ - return path(pathname.c_str()); -} +std::string vpIoTools::path(const std::string &pathname) { return path(pathname.c_str()); } /*! Reads the configuration file and parses it. @@ -1154,9 +1087,7 @@ bool vpIoTools::readConfigVar(const std::string &var, std::string &value) \return true if the parameter could be read. */ -bool vpIoTools::readConfigVar(const std::string &var, - vpArray2D &value, - const unsigned int &nCols, +bool vpIoTools::readConfigVar(const std::string &var, vpArray2D &value, const unsigned int &nCols, const unsigned int &nRows) { bool found = false; @@ -1199,8 +1130,7 @@ bool vpIoTools::readConfigVar(const std::string &var, \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) +void vpIoTools::addNameElement(const std::string &strTrue, const bool &cond, const std::string &strFalse) { if (cond) baseName += "_" + strTrue; @@ -1282,8 +1212,7 @@ std::string vpIoTools::getViSPImagesDataPath() std::string data_path; std::string file_to_test("mbt/cube.cao"); std::string filename; -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#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/ViSP-images"; filename = data_path + "/" + file_to_test; @@ -1325,11 +1254,9 @@ std::string vpIoTools::getViSPImagesDataPath() 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) +std::string vpIoTools::getFileExtension(const std::string &pathname, const bool checkFile) { - if (checkFile && (vpIoTools::checkDirectory(pathname) || - !vpIoTools::checkFilename(pathname))) { + if (checkFile && (vpIoTools::checkDirectory(pathname) || !vpIoTools::checkFilename(pathname))) { return ""; } @@ -1383,8 +1310,7 @@ std::string vpIoTools::getFileExtension(const std::string &pathname, 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 && (int)dotIndex > sepIndex) || sepIndex == (int)std::string::npos) { if (sepIndex == (int)std::string::npos) { sepIndex = -1; } @@ -1467,8 +1393,7 @@ std::string vpIoTools::getParent(const std::string &pathname) std::string vpIoTools::getAbsolutePathname(const std::string &pathname) { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX std::string real_path_str = pathname; char *real_path = realpath(pathname.c_str(), NULL); @@ -1507,8 +1432,7 @@ std::string vpIoTools::getAbsolutePathname(const std::string &pathname) into "\\" characters. Under Unix systems all the "\\" characters are converted into "/" characters. */ -std::string vpIoTools::createFilePath(const std::string &parent, - const std::string &child) +std::string vpIoTools::createFilePath(const std::string &parent, const std::string &child) { if (child.size() == 0 && parent.size() == 0) { return ""; @@ -1530,8 +1454,7 @@ std::string vpIoTools::createFilePath(const std::string &parent, std::string stringSeparator; ss >> stringSeparator; - std::string lastConvertedParentChar = - convertedParent.substr(convertedParent.size() - 1); + std::string lastConvertedParentChar = convertedParent.substr(convertedParent.size() - 1); std::string firstConvertedChildChar = convertedChild.substr(0, 1); if (lastConvertedParentChar == stringSeparator) { @@ -1557,15 +1480,14 @@ bool vpIoTools::isAbsolutePathname(const std::string &pathname) //# 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. + // 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) == "\\"); + return path.size() > 0 && (path.substr(0, 1) == "/" || path.substr(0, 1) == "\\"); } /*! @@ -1575,8 +1497,7 @@ bool vpIoTools::isAbsolutePathname(const std::string &pathname) \note It uses path() to normalize the path and getAbsolutePathname() to get the absolute pathname. */ -bool vpIoTools::isSamePathname(const std::string &pathname1, - const std::string &pathname2) +bool vpIoTools::isSamePathname(const std::string &pathname1, const std::string &pathname2) { // Normalize path std::string path1_normalize = vpIoTools::path(pathname1); @@ -1596,8 +1517,7 @@ bool vpIoTools::isSamePathname(const std::string &pathname1, element is the drive specification and the second element the path specification */ -std::pair -vpIoTools::splitDrive(const std::string &pathname) +std::pair vpIoTools::splitDrive(const std::string &pathname) { //# Split a path in a drive specification (a drive letter followed by a //# colon) and the path specification. @@ -1648,10 +1568,9 @@ vpIoTools::splitDrive(const std::string &pathname) // 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__))) +// 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("", pathname); #else const std::string sep = "\\"; @@ -1660,11 +1579,9 @@ vpIoTools::splitDrive(const std::string &pathname) if (pathname.size() > 1) { std::string normPathname = pathname; - std::replace(normPathname.begin(), normPathname.end(), *altsep.c_str(), - *sep.c_str()); + std::replace(normPathname.begin(), normPathname.end(), *altsep.c_str(), *sep.c_str()); - if (normPathname.substr(0, 2) == sepsep && - normPathname.substr(2, 1) != sep) { + 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\... @@ -1685,13 +1602,11 @@ vpIoTools::splitDrive(const std::string &pathname) index2 = pathname.size(); } - return std::pair(pathname.substr(0, index2), - pathname.substr(index2)); + return std::pair(pathname.substr(0, index2), pathname.substr(index2)); } if (normPathname[1] == ':') { - return std::pair(pathname.substr(0, 2), - pathname.substr(2)); + return std::pair(pathname.substr(0, 2), pathname.substr(2)); } } @@ -1747,8 +1662,7 @@ other example \endcode */ -std::vector vpIoTools::splitChain(const std::string &chain, - const std::string &sep) +std::vector vpIoTools::splitChain(const std::string &chain, const std::string &sep) { size_t startIndex = 0; @@ -1781,21 +1695,17 @@ std::vector vpIoTools::getDirFiles(const std::string &pathname) { if (!checkDirectory(pathname)) { - throw(vpIoException(vpException::fatalError, - "Directory %s doesn't exist'", pathname.c_str())); + throw(vpIoException(vpException::fatalError, "Directory %s doesn't exist'", pathname.c_str())); } std::string dirName = path(pathname); -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX std::vector files; struct dirent **list = NULL; int filesCount = scandir(dirName.c_str(), &list, NULL, NULL); if (filesCount == -1) { - throw(vpIoException(vpException::fatalError, - "Cannot read files of directory %s", - dirName.c_str())); + throw(vpIoException(vpException::fatalError, "Cannot read files of directory %s", dirName.c_str())); } for (int i = 0; i < filesCount; i++) { std::string fileName = list[i]->d_name; @@ -1821,9 +1731,7 @@ std::vector vpIoTools::getDirFiles(const std::string &pathname) return files; } if (hFind == INVALID_HANDLE_VALUE) { - throw(vpIoException(vpException::fatalError, - "Cannot read files of directory %s", - dirName.c_str())); + throw(vpIoException(vpException::fatalError, "Cannot read files of directory %s", dirName.c_str())); } do { std::string fileName = FindFileData.cFileName; diff --git a/modules/core/src/tools/geometry/vpPlane.cpp b/modules/core/src/tools/geometry/vpPlane.cpp index 0e877bc7d4..1754203421 100644 --- a/modules/core/src/tools/geometry/vpPlane.cpp +++ b/modules/core/src/tools/geometry/vpPlane.cpp @@ -75,11 +75,7 @@ vpPlane::vpPlane() : A(0), B(0), C(0), D(0) {} \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) -{ -} +vpPlane::vpPlane(const double a, const double b, const double c, const double d) : A(a), B(b), C(c), D(d) {} /*! Copy constructor. @@ -111,8 +107,7 @@ vpPlane::vpPlane(const vpPlane &P) : A(0), B(0), C(0), D(0) 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) +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]; @@ -170,8 +165,7 @@ void vpPlane::init(const vpColVector &P, const vpColVector &n) coordinates expressed in the camera or object frame. */ -void vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, - vpPlaneFrame frame) +void vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame) { vpColVector a(3); vpColVector b(3); @@ -228,9 +222,7 @@ void vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, \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) +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); } @@ -294,8 +286,7 @@ void vpPlane::projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const Pproj.set_W(1); } -double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, - vpColVector &H) const +double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVector &H) const { double k, scal; @@ -312,9 +303,7 @@ double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, scal = getA() * R[0] + getB() * R[1] + getC() * R[2]; // if (scal != 0) if (std::fabs(scal) > std::numeric_limits::epsilon()) - k = -(getA() * M0.get_X() + getB() * M0.get_Y() + getC() * M0.get_Z() + - getD()) / - scal; + k = -(getA() * M0.get_X() + getB() * M0.get_Y() + getC() * M0.get_Z() + getD()) / scal; else k = 0; @@ -380,6 +369,5 @@ void vpPlane::changeFrame(const vpHomogeneousMatrix &cMo) */ VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpPlane &p) { - return (os << "(" << p.getA() << "," << p.getB() << "," << p.getC() << "," - << p.getD() << ") "); + 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 index 18f9420b74..354d03e698 100644 --- a/modules/core/src/tools/geometry/vpPolygon.cpp +++ b/modules/core/src/tools/geometry/vpPolygon.cpp @@ -52,8 +52,7 @@ \f$ (0,0) \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$. */ vpPolygon::vpPolygon() - : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), - m_PnPolyConstants(), m_PnPolyMultiples() + : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples() { std::vector corners; corners.push_back(vpImagePoint(0, 0)); @@ -71,8 +70,7 @@ vpPolygon::vpPolygon() \param corners : The Points defining the corners. */ vpPolygon::vpPolygon(const std::vector &corners) - : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), - m_PnPolyConstants(), m_PnPolyMultiples() + : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples() { if (corners.size() < 3) { _goodPoly = false; @@ -89,8 +87,7 @@ vpPolygon::vpPolygon(const std::vector &corners) \param corners : The Points defining the corners. */ vpPolygon::vpPolygon(const std::list &corners) - : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), - m_PnPolyConstants(), m_PnPolyMultiples() + : _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples() { if (corners.size() < 3) { _goodPoly = false; @@ -104,10 +101,8 @@ vpPolygon::vpPolygon(const std::list &corners) \param poly : The polygon used for the initialisation. */ vpPolygon::vpPolygon(const vpPolygon &poly) - : _corners(poly._corners), _center(poly._center), _area(poly._area), - _goodPoly(poly._goodPoly), _bbox(poly._bbox), - m_PnPolyConstants(poly.m_PnPolyConstants), - m_PnPolyMultiples(poly.m_PnPolyMultiples) + : _corners(poly._corners), _center(poly._center), _area(poly._area), _goodPoly(poly._goodPoly), _bbox(poly._bbox), + m_PnPolyConstants(poly.m_PnPolyConstants), m_PnPolyMultiples(poly.m_PnPolyMultiples) { } @@ -141,10 +136,7 @@ vpPolygon &vpPolygon::operator=(const vpPolygon &poly) \param corners : The corners of the polyon. */ -void vpPolygon::buildFrom(const std::vector &corners) -{ - init(corners); -} +void vpPolygon::buildFrom(const std::vector &corners) { init(corners); } /*! Initialises the polygon thanks to the collection of 2D points (in pixel). @@ -154,10 +146,7 @@ void vpPolygon::buildFrom(const std::vector &corners) \param corners : The corners of the polyon. */ -void vpPolygon::buildFrom(const std::list &corners) -{ - init(corners); -} +void vpPolygon::buildFrom(const std::list &corners) { init(corners); } /*! Initialises the triangle thanks to the collection of 2D points (in meter). @@ -171,13 +160,11 @@ void vpPolygon::buildFrom(const std::list &corners) \param cam : The camera parameters used to convert the coordinates from meter to pixel. */ -void vpPolygon::buildFrom(const std::vector &corners, - const vpCameraParameters &cam) +void vpPolygon::buildFrom(const std::vector &corners, const vpCameraParameters &cam) { std::vector 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]); + vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]); } buildFrom(ipCorners); } @@ -191,8 +178,8 @@ void vpPolygon::buildFrom(const std::vector &corners, a polygon corner. \param color : Color used to display the cross over the polygon corner. \param thickness : Thickness used to display the cross. */ -void vpPolygon::initClick(const vpImage &I, unsigned int size, - const vpColor &color, unsigned int thickness) +void vpPolygon::initClick(const vpImage &I, unsigned int size, const vpColor &color, + unsigned int thickness) { vpMouseButton::vpMouseButtonType button = vpMouseButton::button1; vpImagePoint ip; @@ -221,8 +208,7 @@ void vpPolygon::initClick(const vpImage &I, unsigned int size, cross over the polygon corner. \param thickness : Thickness used to display the cross. */ -void vpPolygon::initClick(const vpImage &I, unsigned int size, - const vpColor &color, unsigned int thickness) +void vpPolygon::initClick(const vpImage &I, unsigned int size, const vpColor &color, unsigned int thickness) { vpMouseButton::vpMouseButtonType button = vpMouseButton::button1; vpImagePoint ip; @@ -292,9 +278,7 @@ void vpPolygon::init(const std::list &corners) \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, +bool vpPolygon::testIntersectionSegments(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpImagePoint &ip3, const vpImagePoint &ip4) const { double di1 = ip2.get_i() - ip1.get_i(); @@ -306,20 +290,15 @@ bool vpPolygon::testIntersectionSegments(const vpImagePoint &ip1, double denominator = di1 * dj2 - dj1 * di2; if (fabs(denominator) < std::numeric_limits::epsilon()) { - throw vpException(vpException::divideByZeroError, - "Denominator is null, lines are parallels"); + 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; + 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; + double beta = -(di1 * (ip3.get_j() - ip1.get_j()) + dj1 * (ip1.get_i() - ip3.get_i())) / denominator; if (beta < 0 || beta >= 1) { return false; } @@ -335,8 +314,7 @@ bool vpPolygon::testIntersectionSegments(const vpImagePoint &ip1, \return Returns true if the point is inside the polygon, false otherwise. */ -bool vpPolygon::isInside(const vpImagePoint &ip, - const PointInPolygonMethod &method) const +bool vpPolygon::isInside(const vpImagePoint &ip, const PointInPolygonMethod &method) const { if (_corners.size() < 3) { return false; @@ -348,12 +326,11 @@ 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). + 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). bool oddNbIntersections = false; for (unsigned int i = 0; i < _corners.size(); ++i) { @@ -385,13 +362,9 @@ bool vpPolygon::isInside(const vpImagePoint &ip, default: { bool oddNodes = false; for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) { - if ((_corners[i].get_v() < ip.get_v() && - _corners[j].get_v() >= ip.get_v()) || - (_corners[j].get_v() < ip.get_v() && - _corners[i].get_v() >= ip.get_v())) { - oddNodes ^= - (ip.get_v() * m_PnPolyMultiples[i] + m_PnPolyConstants[i] < - ip.get_u()); + if ((_corners[i].get_v() < ip.get_v() && _corners[j].get_v() >= ip.get_v()) || + (_corners[j].get_v() < ip.get_v() && _corners[i].get_v() >= ip.get_v())) { + oddNodes ^= (ip.get_v() * m_PnPolyMultiples[i] + m_PnPolyConstants[i] < ip.get_u()); } j = i; @@ -414,18 +387,14 @@ void vpPolygon::precalcValuesPnPoly() m_PnPolyMultiples.resize(_corners.size()); for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) { - if (vpMath::equal(_corners[j].get_v(), _corners[i].get_v(), - std::numeric_limits::epsilon())) { + if (vpMath::equal(_corners[j].get_v(), _corners[i].get_v(), std::numeric_limits::epsilon())) { m_PnPolyConstants[i] = _corners[i].get_u(); m_PnPolyMultiples[i] = 0.0; } else { m_PnPolyConstants[i] = _corners[i].get_u() - - (_corners[i].get_v() * _corners[j].get_u()) / - (_corners[j].get_v() - _corners[i].get_v()) + - (_corners[i].get_v() * _corners[i].get_u()) / - (_corners[j].get_v() - _corners[i].get_v()); - m_PnPolyMultiples[i] = (_corners[j].get_u() - _corners[i].get_u()) / - (_corners[j].get_v() - _corners[i].get_v()); + (_corners[i].get_v() * _corners[j].get_u()) / (_corners[j].get_v() - _corners[i].get_v()) + + (_corners[i].get_v() * _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v()); + m_PnPolyMultiples[i] = (_corners[j].get_u() - _corners[i].get_u()) / (_corners[j].get_v() - _corners[i].get_v()); } j = i; @@ -452,8 +421,7 @@ void vpPolygon::updateArea() 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 += _corners[i].get_j() * _corners[i_p_1].get_i() - _corners[i_p_1].get_j() * _corners[i].get_i(); } _area /= 2; @@ -494,12 +462,10 @@ void vpPolygon::updateCenter() 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()); + (_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()); + (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i()); } #endif @@ -554,13 +520,11 @@ void vpPolygon::updateBoundingBox() \param color : The color of the polygon's lines. \param thickness : The thickness of the polygon's lines. */ -void vpPolygon::display(const vpImage &I, const vpColor &color, - unsigned int thickness) const +void vpPolygon::display(const vpImage &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); + vpDisplay::displayLine(I, _corners[i], _corners[(i + 1) % N], color, thickness); } } @@ -575,8 +539,7 @@ void vpPolygon::display(const vpImage &I, const vpColor &color, \return True if the point defined by (i,j) is inside the polygon, False otherwise. */ -bool vpPolygon::isInside(const std::vector &roi, - const double &i, const double &j, +bool vpPolygon::isInside(const std::vector &roi, const double &i, const double &j, const PointInPolygonMethod &method) { vpPolygon poly(roi); @@ -586,7 +549,4 @@ bool vpPolygon::isInside(const std::vector &roi, /*! Return number of corners belonging to the polygon. */ -unsigned int vpPolygon::getSize() const -{ - return ((unsigned int)_corners.size()); -} +unsigned int vpPolygon::getSize() const { return ((unsigned int)_corners.size()); } diff --git a/modules/core/src/tools/geometry/vpPolygon3D.cpp b/modules/core/src/tools/geometry/vpPolygon3D.cpp index 44df28b7cc..52876d4be5 100644 --- a/modules/core/src/tools/geometry/vpPolygon3D.cpp +++ b/modules/core/src/tools/geometry/vpPolygon3D.cpp @@ -51,16 +51,14 @@ Basic constructor. */ vpPolygon3D::vpPolygon3D() - : nbpt(0), nbCornersInsidePrev(0), p(NULL), polyClipped(), - clippingFlag(vpPolygon3D::NO_CLIPPING), distNearClip(0.001), - distFarClip(100.) + : 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) + : nbpt(mbtp.nbpt), nbCornersInsidePrev(mbtp.nbCornersInsidePrev), p(NULL), polyClipped(mbtp.polyClipped), + clippingFlag(mbtp.clippingFlag), distNearClip(mbtp.distNearClip), distFarClip(mbtp.distFarClip) { if (p) delete[] p; @@ -171,82 +169,71 @@ void vpPolygon3D::computePolygonClipped(const vpCameraParameters &cam) for (unsigned int i = 0; i < nbpt; i++) { p[i % nbpt].projection(); - polyClippedTemp.push_back( - std::make_pair(p[i % nbpt], vpPolygon3D::NO_CLIPPING)); + 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))) { - if (i > vpPolygon3D::FAR_CLIPPING && - !cam.isFovComputed()) // To make sure we do not compute FOV - // clipping if camera normals are not - // computed + if (((clippingFlag & i) == i) || ((clippingFlag > vpPolygon3D::FAR_CLIPPING) && (i == 1))) { + if (i > vpPolygon3D::FAR_CLIPPING && !cam.isFovComputed()) // To make sure we do not compute FOV + // clipping if camera normals are not + // computed continue; for (unsigned int j = 0; j < polyClippedTemp.size(); j++) { vpPoint p1Clipped = polyClippedTemp[j].first; - vpPoint p2Clipped = - polyClippedTemp[(j + 1) % polyClippedTemp.size()].first; + vpPoint p2Clipped = polyClippedTemp[(j + 1) % polyClippedTemp.size()].first; - unsigned int p2ClippedInfoBefore = - polyClippedTemp[(j + 1) % polyClippedTemp.size()].second; + unsigned int p2ClippedInfoBefore = polyClippedTemp[(j + 1) % polyClippedTemp.size()].second; unsigned int p1ClippedInfo = polyClippedTemp[j].second; - unsigned int p2ClippedInfo = - polyClippedTemp[(j + 1) % polyClippedTemp.size()].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)); + 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)); + 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)); + 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)); + 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)); + 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)); + 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)); + polyClippedTemp2.push_back(std::make_pair(p1Clipped, p1ClippedInfo)); if (p2ClippedInfo != p2ClippedInfoBefore) { p2Clipped.projection(); - polyClippedTemp2.push_back( - std::make_pair(p2Clipped, p2ClippedInfo)); + polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo)); } if (nbpt == 2) { if (p2ClippedInfo == p2ClippedInfoBefore) { p2Clipped.projection(); - polyClippedTemp2.push_back( - std::make_pair(p2Clipped, p2ClippedInfo)); + polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo)); } break; } @@ -280,11 +267,10 @@ void vpPolygon3D::computePolygonClipped(const vpCameraParameters &cam) \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) +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(); @@ -309,10 +295,8 @@ bool vpPolygon3D::getClippedPointsFovGeneric( 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()) + + 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()); @@ -332,11 +316,9 @@ bool vpPolygon3D::getClippedPointsFovGeneric( 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) +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, @@ -369,10 +351,8 @@ bool vpPolygon3D::getClippedPointsDistance( 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_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) { @@ -422,8 +402,7 @@ std::vector vpPolygon3D::getRoi(const vpCameraParameters &cam) \return Image point corresponding to the region of interest. */ -std::vector vpPolygon3D::getRoi(const vpCameraParameters &cam, - const vpHomogeneousMatrix &cMo) +std::vector vpPolygon3D::getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo) { changeFrame(cMo); return getRoi(cam); @@ -454,11 +433,7 @@ void vpPolygon3D::getRoiClipped(std::vector &points) \param poly : resulting points plus clipping information. */ -void vpPolygon3D::getPolygonClipped( - std::vector > &poly) -{ - poly = polyClipped; -} +void vpPolygon3D::getPolygonClipped(std::vector > &poly) { poly = polyClipped; } /*! Get the 3D clipped points. @@ -484,13 +459,11 @@ void vpPolygon3D::getPolygonClipped(std::vector &poly) \param cam : camera parameters. \param roi : image point corresponding to the region of interest. */ -void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, - std::vector &roi) +void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector &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); + 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); @@ -504,8 +477,7 @@ void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, \param cMo : pose. \param roi : image point corresponding to the region of interest. */ -void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, - std::vector &roi, +void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector &roi, const vpHomogeneousMatrix &cMo) { changeFrame(cMo); @@ -524,15 +496,12 @@ void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, \param roi : image point corresponding to the region of interest with clipping information. */ -void vpPolygon3D::getRoiClipped( - const vpCameraParameters &cam, - std::vector > &roi) +void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector > &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); + vpMeterPixelConversion::convertPoint(cam, polyClipped[i].first.get_x(), polyClipped[i].first.get_y(), ip); roi.push_back(std::make_pair(ip, polyClipped[i].second)); } } @@ -545,10 +514,8 @@ void vpPolygon3D::getRoiClipped( \param roi : image point corresponding to the region of interest with clipping information. \param cMo : pose. */ -void vpPolygon3D::getRoiClipped( - const vpCameraParameters &cam, - std::vector > &roi, - const vpHomogeneousMatrix &cMo) +void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector > &roi, + const vpHomogeneousMatrix &cMo) { changeFrame(cMo); computePolygonClipped(cam); @@ -562,18 +529,14 @@ void vpPolygon3D::getRoiClipped( \param I : The image used for its size. \param cam : The camera parameters. */ -unsigned int -vpPolygon3D::getNbCornerInsideImage(const vpImage &I, - const vpCameraParameters &cam) +unsigned int vpPolygon3D::getNbCornerInsideImage(const vpImage &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())) + 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++; } } @@ -603,24 +566,19 @@ vpPolygon3D::getNbCornerInsideImage(const vpImage &I, zfar : Far clipping distance value (Only used if clipping flags contain Far clipping). */ -void vpPolygon3D::getClippedPolygon(const std::vector &ptIn, - std::vector &ptOut, - const vpHomogeneousMatrix &cMo, - const unsigned int &clippingFlags, - const vpCameraParameters &cam, - const double &znear, const double &zfar) +void vpPolygon3D::getClippedPolygon(const std::vector &ptIn, std::vector &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) + if ((clippingFlags & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING) poly.setNearClippingDistance(znear); - if ((clippingFlags & vpPolygon3D::FAR_CLIPPING) == - vpPolygon3D::FAR_CLIPPING) + if ((clippingFlags & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING) poly.setFarClippingDistance(zfar); for (unsigned int i = 0; i < ptIn.size(); i++) @@ -631,8 +589,7 @@ void vpPolygon3D::getClippedPolygon(const std::vector &ptIn, poly.getPolygonClipped(ptOut); } -void vpPolygon3D::getMinMaxRoi(const std::vector &iroi, - int &i_min, int &i_max, int &j_min, int &j_max) +void vpPolygon3D::getMinMaxRoi(const std::vector &iroi, int &i_min, int &i_max, int &j_min, int &j_max) { // i_min_d = std::numeric_limits::max(); // create an error under // Windows. To fix it we have to add #undef max @@ -673,13 +630,11 @@ void vpPolygon3D::getMinMaxRoi(const std::vector &iroi, \param I : The image used for its size. \param corners : The vector of points defining a region */ -bool vpPolygon3D::roiInsideImage(const vpImage &I, - const std::vector &corners) +bool vpPolygon3D::roiInsideImage(const vpImage &I, const std::vector &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()) && + if ((corners[i].get_i() >= 0) && (corners[i].get_j() >= 0) && (corners[i].get_i() < I.getHeight()) && (corners[i].get_j() < I.getWidth())) { nbPolyIn++; } diff --git a/modules/core/src/tools/geometry/vpRect.cpp b/modules/core/src/tools/geometry/vpRect.cpp index 0dc1260b6b..60f40b1c83 100644 --- a/modules/core/src/tools/geometry/vpRect.cpp +++ b/modules/core/src/tools/geometry/vpRect.cpp @@ -61,10 +61,7 @@ vpRect::vpRect() : left(0), top(0), width(0), height(0) {} \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) -{ -} +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 @@ -96,10 +93,7 @@ vpRect::vpRect(const vpImagePoint &topLeft, const vpImagePoint &bottomRight) /*! 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; -} +vpRect::vpRect(const vpRect &r) : left(0), top(0), width(0), height(0) { *this = r; } /*! @@ -119,11 +113,7 @@ vpRect &vpRect::operator=(const vpRect &r) \param ip : Vector of image points. At least 1 points is mandatory, otherwise an exception is thrown. */ -vpRect::vpRect(const std::vector &ip) - : left(0), top(0), width(0), height(0) -{ - set(ip); -} +vpRect::vpRect(const std::vector &ip) : left(0), top(0), width(0), height(0) { set(ip); } /*! Check if an image point belongs to the rectangle. @@ -134,8 +124,8 @@ vpRect::vpRect(const std::vector &ip) */ bool vpRect::isInside(const vpImagePoint &ip) const { - return (ip.get_i() <= this->getBottom() && ip.get_i() >= this->getTop() && - ip.get_j() <= this->getRight() && ip.get_j() >= this->getLeft()); + return (ip.get_i() <= this->getBottom() && ip.get_i() >= this->getTop() && ip.get_j() <= this->getRight() && + ip.get_j() >= this->getLeft()); } /*! @@ -178,8 +168,7 @@ void vpRect::set(const vpImagePoint &topLeft, double w, double h) void vpRect::set(const std::vector &ip) { if (ip.size() < 1) - throw(vpException(vpException::dimensionError, - "At least 1 point is requested to build a rectangle")); + 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(); @@ -233,14 +222,10 @@ bool vpRect::operator==(const vpRect &r) const { // return (top == r.top && left == r.left && width == r.width && height == // r.height); - return (std::fabs(top - r.top) <= - std::fabs(top) * std::numeric_limits::epsilon() && - std::fabs(left - r.left) <= - std::fabs(left) * std::numeric_limits::epsilon() && - std::fabs(width - r.width) <= - std::fabs(width) * std::numeric_limits::epsilon() && - std::fabs(height - r.height) <= - std::fabs(height) * std::numeric_limits::epsilon()); + return (std::fabs(top - r.top) <= std::fabs(top) * std::numeric_limits::epsilon() && + std::fabs(left - r.left) <= std::fabs(left) * std::numeric_limits::epsilon() && + std::fabs(width - r.width) <= std::fabs(width) * std::numeric_limits::epsilon() && + std::fabs(height - r.height) <= std::fabs(height) * std::numeric_limits::epsilon()); } /*! @@ -304,13 +289,12 @@ vpRect vpRect::operator&(const vpRect &r) const */ 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()); + 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(); + 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 index a75e15bd78..107aa34f45 100644 --- a/modules/core/src/tools/geometry/vpTriangle.cpp +++ b/modules/core/src/tools/geometry/vpTriangle.cpp @@ -47,8 +47,8 @@ \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() + : 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)); } @@ -61,10 +61,9 @@ vpTriangle::vpTriangle() \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() +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); } @@ -75,8 +74,8 @@ vpTriangle::vpTriangle(const vpImagePoint &iP1, const vpImagePoint &iP2, \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() + : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0), ptempo0(0), ptempo1(0), area(0), apex1(), + apex2(), apex3() { *this = tri; } @@ -114,14 +113,12 @@ vpTriangle &vpTriangle::operator=(const vpTriangle &tri) \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) +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) +void vpTriangle::init(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3) { ptempo0 = ptempo1 = 0.; apex1 = iP1; @@ -176,6 +173,5 @@ bool vpTriangle::inTriangle(const vpImagePoint &iP, double threshold) 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); + 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 index 1245f36b11..ff8402a6e9 100644 --- a/modules/core/src/tools/histogram/vpHistogram.cpp +++ b/modules/core/src/tools/histogram/vpHistogram.cpp @@ -63,16 +63,10 @@ struct Histogram_Param_t { unsigned int *m_histogram; const vpImage *m_I; - Histogram_Param_t() - : m_start_index(0), m_end_index(0), m_lut(), m_histogram(NULL), m_I(NULL) - { - } + Histogram_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_histogram(NULL), m_I(NULL) {} - Histogram_Param_t(const unsigned int start_index, - const unsigned int end_index, - const vpImage *const I) - : m_start_index(start_index), m_end_index(end_index), m_lut(), - m_histogram(NULL), m_I(I) + Histogram_Param_t(const unsigned int start_index, const unsigned int end_index, const vpImage *const I) + : m_start_index(start_index), m_end_index(end_index), m_lut(), m_histogram(NULL), m_I(I) { } @@ -172,8 +166,7 @@ vpHistogram::vpHistogram(const vpHistogram &h) : histogram(NULL), size(256) \sa calculate() */ -vpHistogram::vpHistogram(const vpImage &I) - : histogram(NULL), size(256) +vpHistogram::vpHistogram(const vpImage &I) : histogram(NULL), size(256) { init(); @@ -242,9 +235,7 @@ void vpHistogram::init(unsigned size_) \param nbins : Number of bins to compute the histogram. \param nbThreads : Number of threads to use for the computation. */ -void vpHistogram::calculate(const vpImage &I, - const unsigned int nbins, - const unsigned int nbThreads) +void vpHistogram::calculate(const vpImage &I, const unsigned int nbins, const unsigned int nbThreads) { if (size != nbins) { if (histogram != NULL) { @@ -254,10 +245,7 @@ void vpHistogram::calculate(const vpImage &I, 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; + std::cerr << "nbins=" << nbins << " , nbins should be between ]0 ; 256] ; use by default nbins=256" << std::endl; } histogram = new unsigned int[size]; } @@ -311,8 +299,7 @@ void vpHistogram::calculate(const vpImage &I, end_index = start_index + last_step; } - Histogram_Param_t *histogram_param = - new Histogram_Param_t(start_index, end_index, &I); + Histogram_Param_t *histogram_param = new Histogram_Param_t(start_index, end_index, &I); histogram_param->m_histogram = new unsigned int[size]; memset(histogram_param->m_histogram, 0, size * sizeof(unsigned int)); memcpy(histogram_param->m_lut, lut, 256 * sizeof(unsigned int)); @@ -320,9 +307,7 @@ void vpHistogram::calculate(const vpImage &I, histogramParams.push_back(histogram_param); // Start the threads - vpThread *histogram_thread = - new vpThread((vpThread::Fn)computeHistogramThread, - (vpThread::Args)histogram_param); + vpThread *histogram_thread = new vpThread((vpThread::Fn)computeHistogramThread, (vpThread::Args)histogram_param); threadpool.push_back(histogram_thread); } @@ -364,15 +349,13 @@ void vpHistogram::calculate(const vpImage &I, 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 &I, - const vpColor &color, const unsigned int thickness, +void vpHistogram::display(const vpImage &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; + std::cerr << "Image I must have at least width >= 36 && height >= 36 !" << std::endl; return; } @@ -386,8 +369,7 @@ void vpHistogram::display(const vpImage &I, } if (maxValue == 0) { - throw(vpException(vpException::divideByZeroError, - "Cannot display histogram; max value=0")); + throw(vpException(vpException::divideByZeroError, "Cannot display histogram; max value=0")); } // Keep 12 free pixels at the top unsigned int max_height = height - 12; @@ -398,10 +380,8 @@ void vpHistogram::display(const vpImage &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)); + 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); } } @@ -430,8 +410,7 @@ 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")); + throw(vpImageException(vpImageException::notInitializedError, "Histogram array not initialised")); } vpHistogram h; @@ -471,8 +450,7 @@ unsigned vpHistogram::getPeaks(std::list &peaks) { if (histogram == NULL) { vpERROR_TRACE("Histogram array not initialised\n"); - throw(vpImageException(vpImageException::notInitializedError, - "Histogram array not initialised")); + throw(vpImageException(vpImageException::notInitializedError, "Histogram array not initialised")); } int prev_slope; // Previous histogram inclination @@ -488,8 +466,7 @@ unsigned vpHistogram::getPeaks(std::list &peaks) prev_slope = 1; for (unsigned i = 0; i < size - 1; i++) { - int next_slope = (int)histogram[i + 1] - - (int)histogram[i]; // Next histogram inclination + int next_slope = (int)histogram[i + 1] - (int)histogram[i]; // Next histogram inclination // if ((prev_slope < 0) && (next_slope > 0) ) { // sum_level += i; @@ -547,8 +524,7 @@ unsigned vpHistogram::getPeaks(std::list &peaks) - 0: if no peaks were found. */ -unsigned vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peak1, - vpHistogramPeak &peak2) +unsigned vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peak1, vpHistogramPeak &peak2) { std::list peaks; unsigned nbpeaks; // Number of peaks in the histogram (ie local maxima) @@ -571,8 +547,7 @@ unsigned vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peak1, // Parse the peaks list to get the peak with a distance greater // than dist to the highest peak1 = peaks.front(); - for (std::list::const_iterator it = peaks.begin(); - it != peaks.end(); ++it) { + for (std::list::const_iterator it = peaks.begin(); it != peaks.end(); ++it) { vpHistogramPeak p = *it; if (abs(p.getLevel() - peak1.getLevel()) > dist) { // The second peak is found @@ -601,8 +576,7 @@ unsigned vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peak1, \return true if the histogram is bimodal, false otherwise. */ -bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, - vpHistogramPeak &peakr, vpHistogramValey &valey) +bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogramPeak &peakr, vpHistogramValey &valey) { unsigned char *peak; // Local maxima values int prev_slope; // Previous histogram inclination @@ -614,7 +588,7 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, 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) + unsigned int nbpeaks; // Number of peaks in the histogram (ie local maxima) // Init the valey valey.set(0, 0); @@ -626,8 +600,7 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, nbpeaks = 0; prev_slope = 1; for (unsigned i = 0; i < size - 1; i++) { - int next_slope = (int)histogram[i + 1] - - (int)histogram[i]; // Next histogram inclination + int next_slope = (int)histogram[i + 1] - (int)histogram[i]; // Next histogram inclination if (next_slope == 0) continue; // Peak detection @@ -760,8 +733,7 @@ unsigned vpHistogram::getValey(std::list &valey) { if (histogram == NULL) { vpERROR_TRACE("Histogram array not initialised\n"); - throw(vpImageException(vpImageException::notInitializedError, - "Histogram array not initialised")); + throw(vpImageException(vpImageException::notInitializedError, "Histogram array not initialised")); } int prev_slope; // Previous histogram inclination @@ -777,8 +749,7 @@ unsigned vpHistogram::getValey(std::list &valey) prev_slope = -1; for (unsigned i = 0; i < size - 1; i++) { - int next_slope = (int)histogram[i + 1] - - (int)histogram[i]; // Next histogram inclination + int next_slope = (int)histogram[i + 1] - (int)histogram[i]; // Next histogram inclination if ((prev_slope < 0) && (next_slope == 0)) { sum_level += i + 1; @@ -827,9 +798,7 @@ unsigned vpHistogram::getValey(std::list &valey) \return true if a valey was found, false otherwise. */ -bool vpHistogram::getValey(const vpHistogramPeak &peak1, - const vpHistogramPeak &peak2, - vpHistogramValey &valey) +bool vpHistogram::getValey(const vpHistogramPeak &peak1, const vpHistogramPeak &peak2, vpHistogramValey &valey) { // Set the left and right peaks @@ -893,9 +862,7 @@ bool vpHistogram::getValey(const vpHistogramPeak &peak1, \return 0x11 : If two valeys around the peak were found. */ -unsigned vpHistogram::getValey(unsigned char dist, - const vpHistogramPeak &peak, - vpHistogramValey &valeyl, +unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, vpHistogramValey &valeyl, vpHistogramValey &valeyr) { unsigned int ret = 0x11; @@ -1025,8 +992,7 @@ unsigned vpHistogram::getValey(unsigned char dist, mini = peak.getValue(); sumindmini = 0; nbmini = 0; - for (unsigned i = (unsigned int)peak.getLevel() + 1; - i <= (unsigned int)peakr.getLevel(); i++) { + for (unsigned i = (unsigned int)peak.getLevel() + 1; i <= (unsigned int)peakr.getLevel(); i++) { if (histogram[i] < mini) { mini = histogram[i]; nbmini = 1; @@ -1083,10 +1049,7 @@ unsigned vpHistogram::sort(std::list &peaks) \return true if the file could be written, false otherwise. */ -bool vpHistogram::write(const std::string &filename) -{ - return (this->write(filename.c_str())); -} +bool vpHistogram::write(const std::string &filename) { return (this->write(filename.c_str())); } /*! diff --git a/modules/core/src/tools/histogram/vpHistogramPeak.cpp b/modules/core/src/tools/histogram/vpHistogramPeak.cpp index ac9319fb12..1dc8b7816c 100644 --- a/modules/core/src/tools/histogram/vpHistogramPeak.cpp +++ b/modules/core/src/tools/histogram/vpHistogramPeak.cpp @@ -53,19 +53,12 @@ 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) -{ -} +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; -} +vpHistogramPeak::vpHistogramPeak(const vpHistogramPeak &p) : level(0), value(0) { *this = p; } /*! @@ -92,17 +85,13 @@ vpHistogramPeak &vpHistogramPeak::operator=(const vpHistogramPeak &p) \param p : Gray level histogram peak to compar. */ -bool vpHistogramPeak::operator==(const vpHistogramPeak &p) const -{ - return ((level == p.level) && (value == p.value)); -} +bool vpHistogramPeak::operator==(const vpHistogramPeak &p) const { return ((level == p.level) && (value == p.value)); } /*! \relates vpHistogramPeak \brief std::cout a peak */ -VISP_EXPORT std::ostream &operator<<(std::ostream &s, - const vpHistogramPeak &p) +VISP_EXPORT std::ostream &operator<<(std::ostream &s, const vpHistogramPeak &p) { s << (int)p.getLevel() << " " << p.getValue(); diff --git a/modules/core/src/tools/histogram/vpHistogramValey.cpp b/modules/core/src/tools/histogram/vpHistogramValey.cpp index 7113a6b212..1ba2a816fa 100644 --- a/modules/core/src/tools/histogram/vpHistogramValey.cpp +++ b/modules/core/src/tools/histogram/vpHistogramValey.cpp @@ -79,8 +79,7 @@ bool vpHistogramValey::operator==(const vpHistogramValey &v) const \relates vpHistogramValey \brief std::cout a valey */ -VISP_EXPORT std::ostream &operator<<(std::ostream &s, - const vpHistogramValey &v) +VISP_EXPORT std::ostream &operator<<(std::ostream &s, const vpHistogramValey &v) { s << (int)v.getLevel() << " " << v.getValue(); diff --git a/modules/core/src/tools/network/vpClient.cpp b/modules/core/src/tools/network/vpClient.cpp index fa6dc74525..069c768198 100644 --- a/modules/core/src/tools/network/vpClient.cpp +++ b/modules/core/src/tools/network/vpClient.cpp @@ -56,8 +56,7 @@ vpClient::~vpClient() { stop(); } \return True if the connection has been etablished, false otherwise. */ -bool vpClient::connectToHostname(const std::string &hostname, - const unsigned int &port_serv) +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()); @@ -76,8 +75,7 @@ bool vpClient::connectToHostname(const std::string &hostname, serv.socketFileDescriptorReceptor = socket(AF_INET, SOCK_STREAM, 0); -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX if (serv.socketFileDescriptorReceptor < 0) { #else if (serv.socketFileDescriptorReceptor == INVALID_SOCKET) { @@ -88,8 +86,7 @@ bool vpClient::connectToHostname(const std::string &hostname, 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); + 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); @@ -106,15 +103,13 @@ bool vpClient::connectToHostname(const std::string &hostname, \return True if the connection has been etablished, false otherwise. */ -bool vpClient::connectToIP(const std::string &ip, - const unsigned int &port_serv) +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 !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX if (serv.socketFileDescriptorReceptor < 0) { #else if (serv.socketFileDescriptorReceptor == INVALID_SOCKET) { @@ -139,8 +134,7 @@ bool vpClient::connectToIP(const std::string &ip, void vpClient::deconnect(const unsigned int &index) { if (index < receptor_list.size()) { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#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); @@ -155,8 +149,7 @@ void vpClient::deconnect(const unsigned int &index) 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 +#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); @@ -184,8 +177,7 @@ bool vpClient::connectServer(vpNetwork::vpReceptor &serv) std::cout << "Attempt number " << ind << "..." << std::endl; connectionResult = - connect(serv.socketFileDescriptorReceptor, - (sockaddr *)&serv.receptorAddress, serv.receptorAddressSize); + connect(serv.socketFileDescriptorReceptor, (sockaddr *)&serv.receptorAddress, serv.receptorAddressSize); if (connectionResult >= 0) break; @@ -208,8 +200,7 @@ bool vpClient::connectServer(vpNetwork::vpReceptor &serv) // 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))) { + 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; } diff --git a/modules/core/src/tools/network/vpNetwork.cpp b/modules/core/src/tools/network/vpNetwork.cpp index ad5307444b..34d4d53e0f 100644 --- a/modules/core/src/tools/network/vpNetwork.cpp +++ b/modules/core/src/tools/network/vpNetwork.cpp @@ -39,10 +39,9 @@ #include 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) + : 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; #if TARGET_OS_IPHONE @@ -121,9 +120,7 @@ void vpNetwork::removeDecodingRequest(const char *id) 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; + std::cout << id << i << " : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl; } } @@ -214,18 +211,12 @@ int vpNetwork::sendRequestTo(vpRequest &req, const unsigned int &dest) flags = MSG_NOSIGNAL; // Only for Linux #endif -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX - int value = (int)sendto(receptor_list[dest].socketFileDescriptorReceptor, - message.c_str(), message.size(), flags, - (sockaddr *)&receptor_list[dest].receptorAddress, - receptor_list[dest].receptorAddressSize); +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX + int value = (int)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); + 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; @@ -264,8 +255,7 @@ int vpNetwork::sendAndEncodeRequest(vpRequest &req) \return The number of bytes that have been sent, -1 if an error occured. */ -int vpNetwork::sendAndEncodeRequestTo(vpRequest &req, - const unsigned int &dest) +int vpNetwork::sendAndEncodeRequestTo(vpRequest &req, const unsigned int &dest) { req.encode(); return sendRequestTo(req, dest); @@ -308,8 +298,7 @@ std::vector vpNetwork::receiveRequest() \param receptorEmitting : Index of the receptor emitting the message */ -std::vector -vpNetwork::receiveRequestFrom(const unsigned int &receptorEmitting) +std::vector vpNetwork::receiveRequestFrom(const unsigned int &receptorEmitting) { _receiveRequestFrom(receptorEmitting); return _handleRequests(); @@ -407,8 +396,7 @@ std::vector vpNetwork::receiveAndDecodeRequest() \param receptorEmitting : Index of the receptor emitting the message */ -std::vector -vpNetwork::receiveAndDecodeRequestFrom(const unsigned int &receptorEmitting) +std::vector vpNetwork::receiveAndDecodeRequestFrom(const unsigned int &receptorEmitting) { std::vector res = receiveRequestFrom(receptorEmitting); for (unsigned int i = 0; i < res.size(); i++) { @@ -468,8 +456,7 @@ int vpNetwork::receiveAndDecodeRequestOnce() \return The number of bytes received, -1 if an error occured. */ -int vpNetwork::receiveAndDecodeRequestOnceFrom( - const unsigned int &receptorEmitting) +int vpNetwork::receiveAndDecodeRequestOnceFrom(const unsigned int &receptorEmitting) { int res = receiveRequestOnceFrom(receptorEmitting); if (res != -1) @@ -522,8 +509,7 @@ int vpNetwork::_handleFirstRequest() 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 (indStart == std::string::npos && indSep == std::string::npos && indEnd == std::string::npos) { if (currentMessageReceived.size() != 0) currentMessageReceived.clear(); @@ -533,8 +519,7 @@ int vpNetwork::_handleFirstRequest() return -1; } - if (indStart == std::string::npos || indSep == std::string::npos || - indEnd == std::string::npos) + if (indStart == std::string::npos || indSep == std::string::npos || indEnd == std::string::npos) return -1; if (indEnd < indStart) { @@ -585,15 +570,13 @@ int vpNetwork::_handleFirstRequest() std::string param; while (indEndParam != std::string::npos || indEndParam < indEnd) { - param = currentMessageReceived.substr( - (unsigned)indDebParam, (unsigned)(indEndParam - indDebParam)); + 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); + param = currentMessageReceived.substr((unsigned)indDebParam, indEnd - indDebParam); request_list[(unsigned)indRequest]->addParameter(param); currentMessageReceived.erase(indStart, indEnd + end.size()); @@ -682,14 +665,12 @@ int vpNetwork::_receiveRequestOnce() if (i == 0) socketMax = receptor_list[i].socketFileDescriptorReceptor; - FD_SET((unsigned)receptor_list[i].socketFileDescriptorReceptor, - &readFileDescriptor); + 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 value = select((int)socketMax + 1, &readFileDescriptor, NULL, NULL, &tv); int numbytes = 0; if (value == -1) { @@ -701,24 +682,16 @@ int vpNetwork::_receiveRequestOnce() return 0; } else { for (unsigned int i = 0; i < receptor_list.size(); i++) { - if (FD_ISSET( - (unsigned int)receptor_list[i].socketFileDescriptorReceptor, - &readFileDescriptor)) { + 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 = (int)recv(receptor_list[i].socketFileDescriptorReceptor, - buf, max_size_message, 0); +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX + numbytes = (int)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); + 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; + 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; @@ -776,12 +749,9 @@ int vpNetwork::_receiveRequestOnceFrom(const unsigned int &receptorEmitting) FD_ZERO(&readFileDescriptor); socketMax = receptor_list[receptorEmitting].socketFileDescriptorReceptor; - FD_SET((unsigned int)receptor_list[receptorEmitting] - .socketFileDescriptorReceptor, - &readFileDescriptor); + FD_SET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor, &readFileDescriptor); - int value = - select((int)socketMax + 1, &readFileDescriptor, NULL, NULL, &tv); + int value = select((int)socketMax + 1, &readFileDescriptor, NULL, NULL, &tv); int numbytes = 0; if (value == -1) { if (verboseMode) @@ -791,24 +761,16 @@ int vpNetwork::_receiveRequestOnceFrom(const unsigned int &receptorEmitting) // Timeout return 0; } else { - if (FD_ISSET((unsigned int)receptor_list[receptorEmitting] - .socketFileDescriptorReceptor, - &readFileDescriptor)) { + 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 = (int)recv( - receptor_list[receptorEmitting].socketFileDescriptorReceptor, buf, - max_size_message, 0); +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX + numbytes = (int)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); + 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::cout << "Disconnected : " << inet_ntoa(receptor_list[receptorEmitting].receptorAddress.sin_addr) << std::endl; receptor_list.erase(receptor_list.begin() + (int)receptorEmitting); delete[] buf; diff --git a/modules/core/src/tools/network/vpRequest.cpp b/modules/core/src/tools/network/vpRequest.cpp index 64528e9cc6..193e2b836d 100644 --- a/modules/core/src/tools/network/vpRequest.cpp +++ b/modules/core/src/tools/network/vpRequest.cpp @@ -62,10 +62,7 @@ void vpRequest::addParameter(char *params) \param params : std::string representing the message to add. */ -void vpRequest::addParameter(std::string ¶ms) -{ - listOfParams.push_back(params); -} +void vpRequest::addParameter(std::string ¶ms) { listOfParams.push_back(params); } /*! Add messages as parameters of the request. diff --git a/modules/core/src/tools/network/vpServer.cpp b/modules/core/src/tools/network/vpServer.cpp index a7e82b10ba..f24b35713e 100644 --- a/modules/core/src/tools/network/vpServer.cpp +++ b/modules/core/src/tools/network/vpServer.cpp @@ -39,7 +39,7 @@ #include #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin) -#include // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro +#include // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro #endif /*! @@ -48,10 +48,8 @@ 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 + 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) @@ -72,25 +70,20 @@ vpServer::vpServer() : adress(), port(0), started(false), max_clients(10) \param port_serv : server's port. */ -vpServer::vpServer(const int &port_serv) - : adress(), port(0), started(false), max_clients(10) +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 + 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."); + 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_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); @@ -107,10 +100,8 @@ 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 + 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) @@ -132,16 +123,14 @@ vpServer::vpServer(const std::string &adress_serv, const int &port_serv) */ vpServer::~vpServer() { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#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 +#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); @@ -156,15 +145,12 @@ vpServer::~vpServer() 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, +#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); + int bindResult = bind((unsigned)emitter.socketFileDescriptorEmitter, (struct sockaddr *)&emitter.emitterAddress, + serverStructLength); #endif if (bindResult < 0) { @@ -184,16 +170,15 @@ bool vpServer::start() // 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))) { + 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 +#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); @@ -232,8 +217,7 @@ bool vpServer::checkForConnections() FD_SET((unsigned)emitter.socketFileDescriptorEmitter, &readFileDescriptor); for (unsigned int i = 0; i < receptor_list.size(); i++) { - FD_SET((unsigned)receptor_list[i].socketFileDescriptorReceptor, - &readFileDescriptor); + FD_SET((unsigned)receptor_list[i].socketFileDescriptorReceptor, &readFileDescriptor); if (i == 0) socketMax = receptor_list[i].socketFileDescriptorReceptor; @@ -242,33 +226,26 @@ bool vpServer::checkForConnections() socketMax = receptor_list[i].socketFileDescriptorReceptor; } - int value = - select((int)socketMax + 1, &readFileDescriptor, NULL, NULL, &tv); + 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)) { + 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); +#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, + 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 !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX if ((client.socketFileDescriptorReceptor) == -1) #else if ((client.socketFileDescriptorReceptor) == INVALID_SOCKET) @@ -276,32 +253,22 @@ bool vpServer::checkForConnections() 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)); + 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)) { + if (FD_ISSET((unsigned int)receptor_list[i].socketFileDescriptorReceptor, &readFileDescriptor)) { char deco; -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX - ssize_t numbytes = - recv(receptor_list[i].socketFileDescriptorReceptor, &deco, 1, - MSG_PEEK); +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX + ssize_t 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); + 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; + std::cout << "Disconnected : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl; receptor_list.erase(receptor_list.begin() + (int)i); return 0; } diff --git a/modules/core/src/tools/network/vpUDPClient.cpp b/modules/core/src/tools/network/vpUDPClient.cpp index b20f528412..ff72b0576d 100644 --- a/modules/core/src/tools/network/vpUDPClient.cpp +++ b/modules/core/src/tools/network/vpUDPClient.cpp @@ -36,8 +36,7 @@ #include #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #include #include #include @@ -70,8 +69,7 @@ vpUDPClient::vpUDPClient(const std::string &hostname, const int port) vpUDPClient::~vpUDPClient() { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX close(m_socketFileDescriptor); #else closesocket(m_socketFileDescriptor); @@ -84,8 +82,7 @@ void vpUDPClient::init(const std::string &hostname, const int port) #if defined(_WIN32) if (WSAStartup(MAKEWORD(2, 2), &m_wsa) != 0) { std::stringstream ss; - ss << "Failed WSAStartup for the server, error code: " - << WSAGetLastError(); + ss << "Failed WSAStartup for the server, error code: " << WSAGetLastError(); throw vpException(vpException::fatalError, ss.str()); } #endif @@ -97,8 +94,7 @@ void vpUDPClient::init(const std::string &hostname, const int port) #else if (m_socketFileDescriptor < 0) #endif - throw vpException(vpException::fatalError, - "Error opening UDP socket for the client!"); + throw vpException(vpException::fatalError, "Error opening UDP socket for the client!"); /* build the server's Internet address */ memset(&m_serverAddress, 0, sizeof(m_serverAddress)); @@ -113,8 +109,7 @@ void vpUDPClient::init(const std::string &hostname, const int port) hints.ai_socktype = SOCK_DGRAM; hints.ai_protocol = IPPROTO_UDP; - DWORD dwRetval = - getaddrinfo(hostname.c_str(), ss.str().c_str(), &hints, &result); + DWORD dwRetval = getaddrinfo(hostname.c_str(), ss.str().c_str(), &hints, &result); if (dwRetval != 0) { ss.str(""); ss << "getaddrinfo failed with error: " << dwRetval; @@ -165,8 +160,7 @@ int vpUDPClient::receive(std::string &msg, const int timeoutMs) timeout.tv_sec = timeoutMs / 1000; timeout.tv_usec = (timeoutMs % 1000) * 1000; } - int retval = select((int)m_socketFileDescriptor + 1, &s, NULL, NULL, - timeoutMs > 0 ? &timeout : NULL); + int retval = select((int)m_socketFileDescriptor + 1, &s, NULL, NULL, timeoutMs > 0 ? &timeout : NULL); if (retval == -1) { std::cerr << "Error select!" << std::endl; @@ -175,8 +169,7 @@ int vpUDPClient::receive(std::string &msg, const int timeoutMs) if (retval > 0) { /* recvfrom: receive a UDP datagram from the server */ - int length = recvfrom(m_socketFileDescriptor, m_buf, sizeof(m_buf), 0, - (struct sockaddr *)&m_serverAddress, + int length = recvfrom(m_socketFileDescriptor, m_buf, sizeof(m_buf), 0, (struct sockaddr *)&m_serverAddress, (socklen_t *)&m_serverLength); if (length <= 0) { return length < 0 ? -1 : 0; @@ -206,10 +199,13 @@ int vpUDPClient::receive(std::string &msg, const int timeoutMs) server.send(os.str(), hostname, port); \endcode To send directly the byte data (assuming the same integer representation on - the server and the client): \code int val = 1024; char data[sizeof(val)]; + the server and the client): + \code + int val = 1024; + char data[sizeof(val)]; memcpy(data, &val, sizeof(val)); - std::string msg(data, sizeof(val)); //required to avoid the string being - splitted with the first \0 character server.send(msg, hostname, port); + std::string msg(data, sizeof(val)); //required to avoid the string being splitted with the first \0 character + server.send(msg, hostname, port); \endcode */ int vpUDPClient::send(const std::string &msg) @@ -220,12 +216,11 @@ int vpUDPClient::send(const std::string &msg) } /* send the message to the server */ -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX - return sendto(m_socketFileDescriptor, msg.c_str(), msg.size(), 0, - (struct sockaddr *)&m_serverAddress, m_serverLength); +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX + return sendto(m_socketFileDescriptor, msg.c_str(), msg.size(), 0, (struct sockaddr *)&m_serverAddress, + m_serverLength); #else - return sendto(m_socketFileDescriptor, msg.c_str(), (int)msg.size(), 0, - (struct sockaddr *)&m_serverAddress, m_serverLength); + return sendto(m_socketFileDescriptor, msg.c_str(), (int)msg.size(), 0, (struct sockaddr *)&m_serverAddress, + m_serverLength); #endif } diff --git a/modules/core/src/tools/network/vpUDPServer.cpp b/modules/core/src/tools/network/vpUDPServer.cpp index 24f6f111eb..2397bd7e84 100644 --- a/modules/core/src/tools/network/vpUDPServer.cpp +++ b/modules/core/src/tools/network/vpUDPServer.cpp @@ -36,8 +36,7 @@ #include #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #include #include #include @@ -60,8 +59,7 @@ \note The server will listen to all the interfaces (see INADDR_ANY). */ vpUDPServer::vpUDPServer(const int port) - : m_clientAddress(), m_clientLength(0), m_serverAddress(), - m_socketFileDescriptor(0) + : m_clientAddress(), m_clientLength(0), m_serverAddress(), m_socketFileDescriptor(0) #if defined(_WIN32) , m_wsa() @@ -77,8 +75,7 @@ vpUDPServer::vpUDPServer(const int port) \param port : Server port number. */ vpUDPServer::vpUDPServer(const std::string &hostname, const int port) - : m_clientAddress(), m_clientLength(0), m_serverAddress(), - m_socketFileDescriptor(0) + : m_clientAddress(), m_clientLength(0), m_serverAddress(), m_socketFileDescriptor(0) #if defined(_WIN32) , m_wsa() @@ -89,8 +86,7 @@ vpUDPServer::vpUDPServer(const std::string &hostname, const int port) vpUDPServer::~vpUDPServer() { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX close(m_socketFileDescriptor); #else closesocket(m_socketFileDescriptor); @@ -103,8 +99,7 @@ void vpUDPServer::init(const std::string &hostname, const int port) #if defined(_WIN32) if (WSAStartup(MAKEWORD(2, 2), &m_wsa) != 0) { std::stringstream ss; - ss << "Failed WSAStartup for the server, error code: " - << WSAGetLastError(); + ss << "Failed WSAStartup for the server, error code: " << WSAGetLastError(); throw vpException(vpException::fatalError, ss.str()); } #endif @@ -116,23 +111,19 @@ void vpUDPServer::init(const std::string &hostname, const int port) #else if (m_socketFileDescriptor < 0) #endif - throw vpException(vpException::fatalError, - "Error opening UDP socket for the server!"); + throw vpException(vpException::fatalError, "Error opening UDP socket for the server!"); /* setsockopt: Handy debugging trick that lets * us rerun the server immediately after we kill it; * otherwise we have to wait about 20 secs. * Eliminates "ERROR on binding: Address already in use" error. */ -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX int optval = 1; - setsockopt(m_socketFileDescriptor, SOL_SOCKET, SO_REUSEADDR, - (const void *)&optval, sizeof(int)); + setsockopt(m_socketFileDescriptor, SOL_SOCKET, SO_REUSEADDR, (const void *)&optval, sizeof(int)); #else const char optval = 1; - setsockopt(m_socketFileDescriptor, SOL_SOCKET, SO_REUSEADDR, - (const char *)&optval, sizeof(int)); + setsockopt(m_socketFileDescriptor, SOL_SOCKET, SO_REUSEADDR, (const char *)&optval, sizeof(int)); #endif /* build the server's Internet address */ @@ -153,8 +144,7 @@ void vpUDPServer::init(const std::string &hostname, const int port) hints.ai_socktype = SOCK_DGRAM; hints.ai_protocol = IPPROTO_UDP; - DWORD dwRetval = - getaddrinfo(hostname.c_str(), ss.str().c_str(), &hints, &result); + DWORD dwRetval = getaddrinfo(hostname.c_str(), ss.str().c_str(), &hints, &result); if (dwRetval != 0) { ss.str(""); ss << "getaddrinfo failed with error: " << dwRetval; @@ -172,10 +162,8 @@ void vpUDPServer::init(const std::string &hostname, const int port) } /* bind: associate the parent socket with a port */ - if (bind(m_socketFileDescriptor, (struct sockaddr *)&m_serverAddress, - sizeof(m_serverAddress)) < 0) - throw vpException(vpException::fatalError, - "Error on binding on the server!"); + if (bind(m_socketFileDescriptor, (struct sockaddr *)&m_serverAddress, sizeof(m_serverAddress)) < 0) + throw vpException(vpException::fatalError, "Error on binding on the server!"); m_clientLength = sizeof(m_clientAddress); } @@ -209,8 +197,7 @@ int vpUDPServer::receive(std::string &msg, const int timeoutMs) there is an error, or 0 if there is a timeout. \note See vpUDPClient::receive for an example. */ -int vpUDPServer::receive(std::string &msg, std::string &hostInfo, - const int timeoutMs) +int vpUDPServer::receive(std::string &msg, std::string &hostInfo, const int timeoutMs) { fd_set s; FD_ZERO(&s); @@ -220,8 +207,7 @@ int vpUDPServer::receive(std::string &msg, std::string &hostInfo, timeout.tv_sec = timeoutMs / 1000; timeout.tv_usec = (timeoutMs % 1000) * 1000; } - int retval = select((int)m_socketFileDescriptor + 1, &s, NULL, NULL, - timeoutMs > 0 ? &timeout : NULL); + int retval = select((int)m_socketFileDescriptor + 1, &s, NULL, NULL, timeoutMs > 0 ? &timeout : NULL); if (retval == -1) { std::cerr << "Error select!" << std::endl; @@ -230,15 +216,12 @@ int vpUDPServer::receive(std::string &msg, std::string &hostInfo, if (retval > 0) { /* recvfrom: receive a UDP datagram from a client */ -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX - int length = recvfrom(m_socketFileDescriptor, m_buf, sizeof(m_buf), 0, - (struct sockaddr *)&m_clientAddress, +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX + int length = recvfrom(m_socketFileDescriptor, m_buf, sizeof(m_buf), 0, (struct sockaddr *)&m_clientAddress, (socklen_t *)&m_clientLength); #else int length = - recvfrom(m_socketFileDescriptor, m_buf, sizeof(m_buf), 0, - (struct sockaddr *)&m_clientAddress, &m_clientLength); + recvfrom(m_socketFileDescriptor, m_buf, sizeof(m_buf), 0, (struct sockaddr *)&m_clientAddress, &m_clientLength); #endif if (length <= 0) { return length < 0 ? -1 : 0; @@ -249,25 +232,21 @@ int vpUDPServer::receive(std::string &msg, std::string &hostInfo, /* getnameinfo: determine who sent the datagram */ char hostname[NI_MAXHOST]; char servInfo[NI_MAXSERV]; - DWORD dwRetval = getnameinfo( - (struct sockaddr *)&m_clientAddress, sizeof(struct sockaddr), - hostname, NI_MAXHOST, servInfo, NI_MAXSERV, NI_NUMERICSERV); + DWORD dwRetval = getnameinfo((struct sockaddr *)&m_clientAddress, sizeof(struct sockaddr), hostname, NI_MAXHOST, + servInfo, NI_MAXSERV, NI_NUMERICSERV); std::string hostName = "", hostIp = "", hostPort = ""; if (dwRetval != 0) { - std::cerr << "getnameinfo failed with error: " << WSAGetLastError() - << std::endl; + std::cerr << "getnameinfo failed with error: " << WSAGetLastError() << std::endl; } else { hostName = hostname; hostPort = servInfo; } char result[INET_ADDRSTRLEN]; - const char *ptr = inet_ntop(AF_INET, (void *)&m_clientAddress.sin_addr, - result, sizeof(result)); + const char *ptr = inet_ntop(AF_INET, (void *)&m_clientAddress.sin_addr, result, sizeof(result)); if (ptr == NULL) { - std::cerr << "inet_ntop failed with error: " << WSAGetLastError() - << std::endl; + std::cerr << "inet_ntop failed with error: " << WSAGetLastError() << std::endl; } else { hostIp = result; } @@ -293,8 +272,7 @@ int vpUDPServer::receive(std::string &msg, std::string &hostInfo, \return The message length / size of the byte array sent. \note See vpUDPClient::send for an example. */ -int vpUDPServer::send(const std::string &msg, const std::string &hostname, - const int port) +int vpUDPServer::send(const std::string &msg, const std::string &hostname, const int port) { if (msg.size() > VP_MAX_UDP_PAYLOAD) { std::cerr << "Message is too long!" << std::endl; @@ -314,8 +292,7 @@ int vpUDPServer::send(const std::string &msg, const std::string &hostname, hints.ai_socktype = SOCK_DGRAM; hints.ai_protocol = IPPROTO_UDP; - DWORD dwRetval = - getaddrinfo(hostname.c_str(), ss.str().c_str(), &hints, &result); + DWORD dwRetval = getaddrinfo(hostname.c_str(), ss.str().c_str(), &hints, &result); if (dwRetval != 0) { ss.str(""); ss << "getaddrinfo failed with error: " << dwRetval; @@ -332,12 +309,11 @@ int vpUDPServer::send(const std::string &msg, const std::string &hostname, freeaddrinfo(result); /* send the message to the client */ -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX - return sendto(m_socketFileDescriptor, msg.c_str(), msg.size(), 0, - (struct sockaddr *)&m_clientAddress, m_clientLength); +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX + return sendto(m_socketFileDescriptor, msg.c_str(), msg.size(), 0, (struct sockaddr *)&m_clientAddress, + m_clientLength); #else - return sendto(m_socketFileDescriptor, msg.c_str(), (int)msg.size(), 0, - (struct sockaddr *)&m_clientAddress, m_clientLength); + return sendto(m_socketFileDescriptor, msg.c_str(), (int)msg.size(), 0, (struct sockaddr *)&m_clientAddress, + m_clientLength); #endif } diff --git a/modules/core/src/tools/time/vpTime.cpp b/modules/core/src/tools/time/vpTime.cpp index 617fd74588..ed2ade1cf5 100644 --- a/modules/core/src/tools/time/vpTime.cpp +++ b/modules/core/src/tools/time/vpTime.cpp @@ -50,8 +50,7 @@ // Unix depend version -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #include #include #elif defined(_WIN32) @@ -99,13 +98,9 @@ double measureTimeMs() return (double)(1000.0 * time.QuadPart / frequency.QuadPart); } #else - throw(vpException( - vpException::fatalError, - "Cannot get time: not implemented on Universal Windows Platform")); + throw(vpException(vpException::fatalError, "Cannot get time: not implemented on Universal Windows Platform")); #endif -#elif !defined(_WIN32) && \ - (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#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); @@ -130,13 +125,9 @@ double measureTimeMicros() return (double)(1000000.0 * time.QuadPart / frequency.QuadPart); } #else - throw(vpException( - vpException::fatalError, - "Cannot get time: not implemented on Universal Windows Platform")); + throw(vpException(vpException::fatalError, "Cannot get time: not implemented on Universal Windows Platform")); #endif -#elif !defined(_WIN32) && \ - (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#elif !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX struct timeval tp; gettimeofday(&tp, 0); return (1000000.0 * tp.tv_sec + tp.tv_usec); @@ -166,11 +157,9 @@ int wait(double t0, double t) if (timeToWait <= 0.) // no need to wait return (1); else { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX if (timeToWait > vpTime::minTimeForUsleepCall) { - usleep( - (useconds_t)((timeToWait - vpTime::minTimeForUsleepCall) * 1000)); + usleep((useconds_t)((timeToWait - vpTime::minTimeForUsleepCall) * 1000)); } // Blocking loop to have an accurate waiting do { @@ -194,9 +183,8 @@ int wait(double t0, double t) return 0; #else - throw(vpException( - vpException::functionNotImplementedError, - "vpTime::wait() is not implemented on Windows Phone 8.0")); + throw(vpException(vpException::functionNotImplementedError, + "vpTime::wait() is not implemented on Windows Phone 8.0")); #endif #endif } @@ -218,12 +206,10 @@ void wait(double t) if (timeToWait <= 0.) // no need to wait return; else { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX double t0 = measureTimeMs(); if (timeToWait > vpTime::minTimeForUsleepCall) { - usleep( - (useconds_t)((timeToWait - vpTime::minTimeForUsleepCall) * 1000)); + usleep((useconds_t)((timeToWait - vpTime::minTimeForUsleepCall) * 1000)); } // Blocking loop to have an accurate waiting do { @@ -248,9 +234,8 @@ void wait(double t) return; #else - throw(vpException( - vpException::functionNotImplementedError, - "vpTime::wait() is not implemented on Windows Phone 8.0")); + throw(vpException(vpException::functionNotImplementedError, + "vpTime::wait() is not implemented on Windows Phone 8.0")); #endif #endif } @@ -272,16 +257,14 @@ double measureTimeSecond() { return vpTime::measureTimeMs() / 1000.0; } */ void sleepMs(double t) { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX usleep((useconds_t)(t * 1000)); #elif defined(_WIN32) #if !defined(WINRT_8_0) Sleep((DWORD)(t)); #else - throw(vpException( - vpException::functionNotImplementedError, - "vpTime::sleepMs() is not implemented on Windows Phone 8.0")); + throw(vpException(vpException::functionNotImplementedError, + "vpTime::sleepMs() is not implemented on Windows Phone 8.0")); #endif #endif } @@ -289,56 +272,48 @@ void sleepMs(double t) /*! Return a string containing date and time. - \param[in] format : The string format supported by strftime() function that -represents the time. The default format is the following \c "%Y/%m/%d -%H:%M:%S". This string contains any combination of special format specifiers -given in the next table: | specifier | Replaced by -| Example | + \param[in] format : The string format supported by strftime() function that represents the time. + The default format is the following \c "%Y/%m/%d %H:%M:%S". + This string contains any combination of special format specifiers given in the next table: + | specifier | Replaced by | Example | |-----------|------------------------------------------------------|--------------------------| - | %%a | Abbreviated weekday name * | Thu -| | %%A | Full weekday name * | -Thursday | | %%b | Abbreviated month name * -| Aug | | %%B | Full month name * -| August | | %%c | Date and time representation * -| Thu Aug 23 14:55:02 2001 | - | %%C | Year divided by 100 and truncated to integer (00-99) | 20 -| | %%d | Day of the month, zero-padded (01-31) | 23 -| | %%D | Short MM/DD/YY date, equivalent to %m/%d/%y | -08/23/01 | - | %%e | Day of the month, space-padded ( 1-31) | 23 -| | %%F | Short YYYY-MM-DD date, equivalent to %Y-%m-%d | -2001-08-23 | - | %%g | Week-based year, last two digits (00-99) | 01 -| | %%G | Week-based year | -2001 | - | %%h | Abbreviated month name * (same as %b) | Aug -| | %%H | Hour in 24h format (00-23) | 14 -| | %%I | Hour in 12h format (01-12) | 02 -| | %%j | Day of the year (001-366) | 235 -| | %%m | Month as a decimal number (01-12) | 08 -| | %%M | Minute (00-59) | 55 -| | %%n | New-line character ('\\n') | -| | %%p | AM or PM designation | PM -| | %%r | 12-hour clock time * | -02:55:02 pm | | %%R | 24-hour HH:MM time, equivalent to -%H:%M | 14:55 | | %%S | Second (00-61) -| 02 | | %%t | Horizontal-tab character ('\\t') -| | | %%T | ISO 8601 time format (HH:MM:SS), -equivalent to %H:%M:%S | 14:55:02 | | %%u | ISO 8601 -weekday as number with Monday as 1 (1-7) | 4 | | %%U -| Week number with the first Sunday as the first day of week one (00-53) | 33 -| | %%V | ISO 8601 week number (00-53) | 34 -| | %%w | Weekday as a decimal number with Sunday as 0 (0-6) | 4 -| | %%W | Week number with the first Monday as the first day of week one -(00-53) | 34 | | %%x | Date representation * -| 08/23/01 | | %%X | Time representation * -| 14:55:02 | | %%y | Year, last two digits (00-99) -| 01 | | %%Y | Year -| 2001 | | %%z | ISO 8601 offset from UTC in -timezone (1 minute=1, 1 hour=100) \n If timezone cannot be determined, no -characters | +100 | | %%Z | Timezone name or abbreviation * \n If -timezone cannot be determined, no characters| CDT | | %% | A % sign -| % | + | %%a | Abbreviated weekday name * | Thu | + | %%A | Full weekday name * | Thursday | + | %%b | Abbreviated month name * | Aug | + | %%B | Full month name * | August | + | %%c | Date and time representation * | Thu Aug 23 14:55:02 2001 | + | %%C | Year divided by 100 and truncated to integer (00-99) | 20 | + | %%d | Day of the month, zero-padded (01-31) | 23 | + | %%D | Short MM/DD/YY date, equivalent to %m/%d/%y | 08/23/01 | + | %%e | Day of the month, space-padded ( 1-31) | 23 | + | %%F | Short YYYY-MM-DD date, equivalent to %Y-%m-%d | 2001-08-23 | + | %%g | Week-based year, last two digits (00-99) | 01 | + | %%G | Week-based year | 2001 | + | %%h | Abbreviated month name * (same as %b) | Aug | + | %%H | Hour in 24h format (00-23) | 14 | + | %%I | Hour in 12h format (01-12) | 02 | + | %%j | Day of the year (001-366) | 235 | + | %%m | Month as a decimal number (01-12) | 08 | + | %%M | Minute (00-59) | 55 | + | %%n | New-line character ('\\n') | | + | %%p | AM or PM designation | PM | + | %%r | 12-hour clock time * | 02:55:02 pm | + | %%R | 24-hour HH:MM time, equivalent to %H:%M | 14:55 | + | %%S | Second (00-61) | 02 | + | %%t | Horizontal-tab character ('\\t') | | + | %%T | ISO 8601 time format (HH:MM:SS), equivalent to %H:%M:%S | 14:55:02 | + | %%u | ISO 8601 weekday as number with Monday as 1 (1-7) | 4 | + | %%U | Week number with the first Sunday as the first day of week one (00-53) | 33 | + | %%V | ISO 8601 week number (00-53) | 34 | + | %%w | Weekday as a decimal number with Sunday as 0 (0-6) | 4 | + | %%W | Week number with the first Monday as the first day of week one (00-53) | 34 | + | %%x | Date representation * | 08/23/01 | + | %%X | Time representation * | 14:55:02 | + | %%y | Year, last two digits (00-99) | 01 | + | %%Y | Year | 2001 | + | %%z | ISO 8601 offset from UTC in timezone (1 minute=1, 1 hour=100) \n If timezone cannot be determined, no characters | +100 | + | %%Z | Timezone name or abbreviation * \n If timezone cannot be determined, no characters| CDT | + | %% | A % sign | % | * The specifiers marked with an asterisk (*) are locale-dependent. \return A formated date and time string. When default format is used, the @@ -350,11 +325,10 @@ returned string contains "YYYY/MM/DD hh:mm:ss". int main() { - std::cout << "%Y/%m/%d %H:%M:%S (default): " << vpTime::getDateTime() << -std::endl; std::cout << "%Y-%m-%d_%H.%M.%S format : " << -vpTime::getDateTime("%Y-%m-%d_%H.%M.%S") << std::endl; std::cout << "%F -format : " << vpTime::getDateTime("%F") << std::endl; std::cout << "%X -format : " << vpTime::getDateTime("%X") << std::endl; + std::cout << "%Y/%m/%d %H:%M:%S (default): " << vpTime::getDateTime() << std::endl; + std::cout << "%Y-%m-%d_%H.%M.%S format : " << vpTime::getDateTime("%Y-%m-%d_%H.%M.%S") << std::endl; + std::cout << "%F format : " << vpTime::getDateTime("%F") << std::endl; + std::cout << "%X format : " << vpTime::getDateTime("%X") << std::endl; return 0; } diff --git a/modules/core/src/tools/xml/vpXmlParser.cpp b/modules/core/src/tools/xml/vpXmlParser.cpp index 5c9327c479..d4de9a3e96 100644 --- a/modules/core/src/tools/xml/vpXmlParser.cpp +++ b/modules/core/src/tools/xml/vpXmlParser.cpp @@ -82,10 +82,7 @@ vpXmlParser::~vpXmlParser() \param _twin : The parser to copy. */ -vpXmlParser::vpXmlParser(const vpXmlParser &_twin) - : nodeMap(_twin.nodeMap), main_tag(_twin.main_tag) -{ -} +vpXmlParser::vpXmlParser(const vpXmlParser &_twin) : nodeMap(_twin.nodeMap), main_tag(_twin.main_tag) {} /* utilities functions to read/write data from an xml document */ @@ -103,8 +100,7 @@ vpXmlParser::vpXmlParser(const vpXmlParser &_twin) 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::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read char*"; std::cerr << errorMsg << std::endl; throw vpException(vpException::fatalError, errorMsg); } @@ -124,8 +120,7 @@ char *vpXmlParser::xmlReadCharChild(xmlDocPtr doc, xmlNodePtr node) 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::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read std::string"; std::cerr << errorMsg << std::endl; throw vpException(vpException::fatalError, errorMsg); } @@ -149,8 +144,7 @@ std::string vpXmlParser::xmlReadStringChild(xmlDocPtr doc, xmlNodePtr 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::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read int"; std::cerr << errorMsg << std::endl; throw vpException(vpException::fatalError, errorMsg); } @@ -181,12 +175,10 @@ int vpXmlParser::xmlReadIntChild(xmlDocPtr doc, xmlNodePtr node) \return the unsigned integer value in the node */ -unsigned int vpXmlParser::xmlReadUnsignedIntChild(xmlDocPtr doc, - xmlNodePtr 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::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read unsigned int"; std::cerr << errorMsg << std::endl; throw vpException(vpException::fatalError, errorMsg); } @@ -220,8 +212,7 @@ unsigned int vpXmlParser::xmlReadUnsignedIntChild(xmlDocPtr doc, 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::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read double"; std::cerr << errorMsg << std::endl; throw vpException(vpException::fatalError, errorMsg); } @@ -254,8 +245,7 @@ double vpXmlParser::xmlReadDoubleChild(xmlDocPtr doc, xmlNodePtr node) float vpXmlParser::xmlReadFloatChild(xmlDocPtr doc, xmlNodePtr node) { if (node->xmlChildrenNode == NULL) { - std::string errorMsg = "Empty node " + std::string((char *)node->name) + - ", cannot read float"; + std::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read float"; std::cerr << errorMsg << std::endl; throw vpException(vpException::fatalError, errorMsg); } @@ -292,8 +282,7 @@ float vpXmlParser::xmlReadFloatChild(xmlDocPtr doc, xmlNodePtr node) bool vpXmlParser::xmlReadBoolChild(xmlDocPtr doc, xmlNodePtr node) { if (node->xmlChildrenNode == NULL) { - std::string errorMsg = "Empty node " + std::string((char *)node->name) + - ", cannot read bool"; + std::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read bool"; std::cerr << errorMsg << std::endl; throw vpException(vpException::fatalError, errorMsg); } @@ -301,9 +290,7 @@ bool vpXmlParser::xmlReadBoolChild(xmlDocPtr doc, xmlNodePtr node) bool val_bool; val_char = (char *)xmlNodeListGetString(doc, node->xmlChildrenNode, 1); - val_bool = - val_char[0] != - '0'; // reading only 1st character : bool xml storage is '0' or '1' + val_bool = val_char[0] != '0'; // reading only 1st character : bool xml storage is '0' or '1' xmlFree((xmlChar *)val_char); return val_bool; @@ -316,8 +303,7 @@ bool vpXmlParser::xmlReadBoolChild(xmlDocPtr doc, xmlNodePtr node) \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) +void vpXmlParser::xmlWriteCharChild(xmlNodePtr node, const char *label, const char *value) { xmlNodePtr tmp; tmp = xmlNewChild(node, NULL, (xmlChar *)label, (xmlChar *)value); @@ -331,8 +317,7 @@ void vpXmlParser::xmlWriteCharChild(xmlNodePtr node, const char *label, \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) +void vpXmlParser::xmlWriteStringChild(xmlNodePtr node, const char *label, const std::string &value) { xmlNodePtr tmp; tmp = xmlNewChild(node, NULL, (xmlChar *)label, (xmlChar *)value.c_str()); @@ -346,8 +331,7 @@ void vpXmlParser::xmlWriteStringChild(xmlNodePtr node, const char *label, \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) +void vpXmlParser::xmlWriteIntChild(xmlNodePtr node, const char *label, const int value) { char str[100]; sprintf(str, "%d", value); @@ -363,8 +347,7 @@ void vpXmlParser::xmlWriteIntChild(xmlNodePtr node, const char *label, \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) +void vpXmlParser::xmlWriteUnsignedIntChild(xmlNodePtr node, const char *label, const unsigned int value) { char str[100]; sprintf(str, "%u", value); @@ -380,8 +363,7 @@ void vpXmlParser::xmlWriteUnsignedIntChild(xmlNodePtr node, const char *label, \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) +void vpXmlParser::xmlWriteDoubleChild(xmlNodePtr node, const char *label, const double value) { char str[100]; sprintf(str, "%lf", value); @@ -397,8 +379,7 @@ void vpXmlParser::xmlWriteDoubleChild(xmlNodePtr node, const char *label, \param label : label (name of the data) of the node \param value : float to write */ -void vpXmlParser::xmlWriteFloatChild(xmlNodePtr node, const char *label, - const float value) +void vpXmlParser::xmlWriteFloatChild(xmlNodePtr node, const char *label, const float value) { char str[100]; sprintf(str, "%f", value); @@ -414,8 +395,7 @@ void vpXmlParser::xmlWriteFloatChild(xmlNodePtr node, const char *label, \param label : label (name of the data) of the node \param value : boolean to write (true or false) */ -void vpXmlParser::xmlWriteBoolChild(xmlNodePtr node, const char *label, - const bool value) +void vpXmlParser::xmlWriteBoolChild(xmlNodePtr node, const char *label, const bool value) { char str[2]; sprintf(str, "%d", (int)value); @@ -475,9 +455,7 @@ 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); + 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()); @@ -486,8 +464,7 @@ void vpXmlParser::save(const std::string &filename, const bool append) if (!append) { xmlFreeDoc(doc); if (remove(filename.c_str()) != 0) - throw vpException(vpException::ioError, - "Cannot remove existing xml file"); + throw vpException(vpException::ioError, "Cannot remove existing xml file"); doc = xmlNewDoc((xmlChar *)"1.0"); root_node = xmlNewNode(NULL, (xmlChar *)main_tag.c_str()); diff --git a/modules/core/src/tracking/forward-projection/vpCircle.cpp b/modules/core/src/tracking/forward-projection/vpCircle.cpp index 0ccbe5d850..4aab5c7d77 100644 --- a/modules/core/src/tracking/forward-projection/vpCircle.cpp +++ b/modules/core/src/tracking/forward-projection/vpCircle.cpp @@ -72,10 +72,8 @@ void vpCircle::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; } \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) +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; @@ -117,8 +115,7 @@ vpCircle::vpCircle(const vpColVector &oP_) \sa setWorldCoordinates() */ -vpCircle::vpCircle(const double A, const double B, const double C, - const double X0, const double Y0, const double Z0, +vpCircle::vpCircle(const double A, const double B, const double C, const double X0, const double Y0, const double Z0, const double R) { init(); @@ -200,8 +197,7 @@ void vpCircle::projection(const vpColVector &cP_, vpColVector &p_) 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 s = 2 * (K[0] * xc * xc + 2 * K[2] * xc * yc + K[1] * yc * yc - K[5]); double A, B, E; @@ -295,25 +291,20 @@ void vpCircle::changeFrame(const vpHomogeneousMatrix &cMo) // vpTRACE("_cP :") ; std::cout << _cP.t() ; } -void vpCircle::display(const vpImage &I, - const vpCameraParameters &cam, const vpColor &color, +void vpCircle::display(const vpImage &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); + 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 &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &color, - const unsigned int thickness) +void vpCircle::display(const vpImage &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); + 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 @@ -338,11 +329,8 @@ vpCircle *vpCircle::duplicate() const 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) +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(); @@ -366,17 +354,14 @@ void vpCircle::computeIntersectionPoint(const vpCircle &circle, 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) / + 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); + 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 index 4a0c641e67..851cb837d2 100644 --- a/modules/core/src/tracking/forward-projection/vpCylinder.cpp +++ b/modules/core/src/tracking/forward-projection/vpCylinder.cpp @@ -68,10 +68,7 @@ void vpCylinder::init() cylinder.setWorldCoordinates(oP); \endcode */ -void vpCylinder::setWorldCoordinates(const vpColVector &o_P) -{ - this->oP = o_P; -} +void vpCylinder::setWorldCoordinates(const vpColVector &o_P) { this->oP = o_P; } /*! Set the cylinder parameters \f$^{o}{\bf P} = @@ -81,10 +78,8 @@ void vpCylinder::setWorldCoordinates(const vpColVector &o_P) \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) +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; @@ -137,9 +132,8 @@ vpCylinder::vpCylinder(const vpColVector &o_P) \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) +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); @@ -215,14 +209,12 @@ void vpCylinder::projection(const vpColVector &cP_, vpColVector &p_) 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 + 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!"); + 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; @@ -264,10 +256,7 @@ void vpCylinder::projection(const vpColVector &cP_, vpColVector &p_) \sa changeFrame(const vpHomogeneousMatrix &, vpColVector &) */ -void vpCylinder::changeFrame(const vpHomogeneousMatrix &cMo) -{ - changeFrame(cMo, cP); -} +void vpCylinder::changeFrame(const vpHomogeneousMatrix &cMo) { changeFrame(cMo, cP); } /*! From the cylinder parameters \f$^{o}{\bf P}\f$ expressed in the world frame, @@ -375,12 +364,9 @@ void vpCylinder::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) */ double vpCylinder::computeZ(const double x, const double y) const { - double A = x * x + y * y + 1 - - ((getA() * x + getB() * y + getC()) * - (getA() * x + getB() * y + getC())); + double A = x * x + y * y + 1 - ((getA() * x + getB() * y + getC()) * (getA() * x + getB() * y + getC())); double B = (x * getX() + y * getY() + getZ()); - double C = - getX() * getX() + getY() * getY() + getZ() * getZ() - getR() * getR(); + double C = getX() * getX() + getY() * getY() + getZ() * getZ() - getR() * getR(); return (B - std::sqrt(B * B - A * C)) / A; } @@ -395,26 +381,21 @@ vpCylinder *vpCylinder::duplicate() const /*! Display the projection of the cylinder in the image as two lines. */ -void vpCylinder::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &color, - const unsigned int thickness) +void vpCylinder::display(const vpImage &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); + 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 &I, - const vpCameraParameters &cam, const vpColor &color, +void vpCylinder::display(const vpImage &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); + vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3], cam, I, color, thickness); } diff --git a/modules/core/src/tracking/forward-projection/vpLine.cpp b/modules/core/src/tracking/forward-projection/vpLine.cpp index 72162f45c7..b80cde2ee8 100644 --- a/modules/core/src/tracking/forward-projection/vpLine.cpp +++ b/modules/core/src/tracking/forward-projection/vpLine.cpp @@ -82,10 +82,8 @@ vpLine::vpLine() { init(); } \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) +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; @@ -119,8 +117,7 @@ void vpLine::setWorldCoordinates(const double &A1, const double &B1, 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"); + throw vpException(vpException::dimensionError, "Size of oP is not equal to 8 as it should be"); this->oP = oP_; } @@ -146,16 +143,13 @@ void vpLine::setWorldCoordinates(const vpColVector &oP_) \left[\begin{array}{c} A2 \\ B2 \\ C2 \\ D2 \end{array}\right] \f] */ -void vpLine::setWorldCoordinates(const vpColVector &oP1, - const vpColVector &oP2) +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"); + 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"); + 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]; @@ -221,8 +215,7 @@ 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"); + 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; @@ -244,9 +237,7 @@ void vpLine::projection(const vpColVector &cP_, vpColVector &p_) 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!"); + throw vpException(vpException::fatalError, "Degenerate case: the image of the straight line is a point!"); } s = 1.0 / sqrt(s); @@ -296,10 +287,7 @@ void vpLine::projection(const vpColVector &cP_, vpColVector &p_) line.changeFrame(cMo); \endcode */ -void vpLine::changeFrame(const vpHomogeneousMatrix &cMo) -{ - changeFrame(cMo, cP); -} +void vpLine::changeFrame(const vpHomogeneousMatrix &cMo) { changeFrame(cMo, cP); } /*! @@ -328,8 +316,7 @@ void vpLine::changeFrame(const vpHomogeneousMatrix &cMo) //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 + //The line is define by the intersection between the plane X = 0.5m and Z = 0.5m //Create the homogeneous matrix vpHomogeneousMatrix cMo; @@ -480,8 +467,7 @@ void vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) \param thickness : Thickness of the feature representation. */ -void vpLine::display(const vpImage &I, - const vpCameraParameters &cam, const vpColor &color, +void vpLine::display(const vpImage &I, const vpCameraParameters &cam, const vpColor &color, const unsigned int thickness) { vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness); @@ -508,10 +494,8 @@ void vpLine::display(const vpImage &I, \param thickness : Thickness of the feature representation. */ // non destructive wrt. cP and p -void vpLine::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &color, - const unsigned int thickness) +void vpLine::display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &color, const unsigned int thickness) { vpColVector _cP, _p; changeFrame(cMo, _cP); diff --git a/modules/core/src/tracking/forward-projection/vpPoint.cpp b/modules/core/src/tracking/forward-projection/vpPoint.cpp index 6e4d9577d4..137c8cf7c2 100644 --- a/modules/core/src/tracking/forward-projection/vpPoint.cpp +++ b/modules/core/src/tracking/forward-projection/vpPoint.cpp @@ -110,8 +110,7 @@ vpPoint::vpPoint(const std::vector &P) 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) +void vpPoint::setWorldCoordinates(const double oX, const double oY, const double oZ) { oP[0] = oX; oP[1] = oY; @@ -142,9 +141,7 @@ void vpPoint::setWorldCoordinates(const vpColVector &P) oP[3] = P[3]; oP /= oP[3]; } else { - throw(vpException(vpException::dimensionError, - "Cannot initialize vpPoint from vector with size %d", - P.size())); + throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", P.size())); } } @@ -171,9 +168,7 @@ void vpPoint::setWorldCoordinates(const std::vector &P) oP[3] = P[3]; oP /= oP[3]; } else { - throw(vpException(vpException::dimensionError, - "Cannot initialize vpPoint from vector with size %d", - P.size())); + throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", P.size())); } } @@ -240,14 +235,10 @@ 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]; + _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; @@ -267,14 +258,10 @@ void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP) */ 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 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; @@ -368,10 +355,8 @@ vpPoint *vpPoint::duplicate() const /*! Display the point in the image. */ -void vpPoint::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &color, - const unsigned int thickness) +void vpPoint::display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &color, const unsigned int thickness) { vpColVector _cP, _p; @@ -387,10 +372,8 @@ void vpPoint::display(const vpImage &I, /*! Display the point in the image. */ -void vpPoint::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &color, - const unsigned int thickness) +void vpPoint::display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &color, const unsigned int thickness) { vpColVector _cP, _p; changeFrame(cMo, _cP); @@ -402,11 +385,7 @@ void vpPoint::display(const vpImage &I, vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness); } -VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpPoint & /* vpp */) -{ - return (os << "vpPoint"); -} +VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPoint & /* vpp */) { return (os << "vpPoint"); } vpPoint &vpPoint::operator=(const vpPoint &vpp) { @@ -421,8 +400,7 @@ vpPoint &vpPoint::operator=(const vpPoint &vpp) /*! Display the point in the image. */ -void vpPoint::display(const vpImage &I, - const vpCameraParameters &cam, const vpColor &color, +void vpPoint::display(const vpImage &I, const vpCameraParameters &cam, const vpColor &color, const unsigned int thickness) { vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness); diff --git a/modules/core/src/tracking/forward-projection/vpSphere.cpp b/modules/core/src/tracking/forward-projection/vpSphere.cpp index fb6c8d5e42..5a2ed0b934 100644 --- a/modules/core/src/tracking/forward-projection/vpSphere.cpp +++ b/modules/core/src/tracking/forward-projection/vpSphere.cpp @@ -50,8 +50,7 @@ void vpSphere::init() void vpSphere::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; } -void vpSphere::setWorldCoordinates(const double X0, const double Y0, - const double Z0, const double R) +void vpSphere::setWorldCoordinates(const double X0, const double Y0, const double Z0, const double R) { oP[0] = X0; oP[1] = Y0; @@ -67,8 +66,7 @@ vpSphere::vpSphere(const vpColVector &oP_) setWorldCoordinates(oP_); } -vpSphere::vpSphere(const double X0, const double Y0, const double Z0, - const double R) +vpSphere::vpSphere(const double X0, const double Y0, const double Z0, const double R) { init(); setWorldCoordinates(X0, Y0, Z0, R); @@ -143,10 +141,7 @@ void vpSphere::projection(const vpColVector &cP_, vpColVector &p_) // std::cout << p.t() ; } //! perspective projection of the circle -void vpSphere::changeFrame(const vpHomogeneousMatrix &cMo) -{ - changeFrame(cMo, cP); -} +void vpSphere::changeFrame(const vpHomogeneousMatrix &cMo) { changeFrame(cMo, cP); } //! Perspective projection of the circle. void vpSphere::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) @@ -172,22 +167,17 @@ vpSphere *vpSphere::duplicate() const } // non destructive wrt. cP and p -void vpSphere::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &color, - const unsigned int thickness) +void vpSphere::display(const vpImage &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); + vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness); } -void vpSphere::display(const vpImage &I, - const vpCameraParameters &cam, const vpColor &color, +void vpSphere::display(const vpImage &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); + 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 index d0824885c0..fa307d20bd 100644 --- a/modules/core/src/tracking/moments/vpMoment.cpp +++ b/modules/core/src/tracking/moments/vpMoment.cpp @@ -54,8 +54,11 @@ 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 #include - #include +the same database +\code +#include +#include +#include #include #include @@ -64,9 +67,8 @@ int main() vpPoint p; std::vector 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) + 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. @@ -96,8 +98,7 @@ image plane (vertex 2) void vpMoment::linkTo(vpMomentDatabase &data_base) { if (strlen(name()) >= 255) { - throw(vpException(vpException::memoryAllocationError, - "Not enough memory to intialize the moment name")); + throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the moment name")); } std::strcpy(_name, name()); @@ -111,10 +112,7 @@ void vpMoment::linkTo(vpMomentDatabase &data_base) values. \param moment_object : object descriptor of the current camera vision. */ -void vpMoment::update(vpMomentObject &moment_object) -{ - this->object = &moment_object; -} +void vpMoment::update(vpMomentObject &moment_object) { this->object = &moment_object; } /*! Prints the moment contents to a stream @@ -123,8 +121,7 @@ void vpMoment::update(vpMomentObject &moment_object) */ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMoment &m) { - for (std::vector::const_iterator i = m.values.begin(); - i != m.values.end(); ++i) + for (std::vector::const_iterator i = m.values.begin(); i != m.values.end(); ++i) os << *i << ","; return os; diff --git a/modules/core/src/tracking/moments/vpMomentAlpha.cpp b/modules/core/src/tracking/moments/vpMomentAlpha.cpp index cae486f5db..9e4927ba7c 100644 --- a/modules/core/src/tracking/moments/vpMomentAlpha.cpp +++ b/modules/core/src/tracking/moments/vpMomentAlpha.cpp @@ -48,11 +48,7 @@ class harbouring an alpha value computed for a \f$[-\pi/2..\pi/2]\f$ portion of the circle. */ -vpMomentAlpha::vpMomentAlpha() - : isRef(true), symmetric(false), ref(), alphaRef(0.) -{ - values.resize(1); -} +vpMomentAlpha::vpMomentAlpha() : isRef(true), symmetric(false), ref(), alphaRef(0.) { values.resize(1); } /*! Common constructor. Initializes alpha moment as a non-reference alpha with a @@ -61,12 +57,10 @@ vpMomentAlpha::vpMomentAlpha() order: \f$\mu_{30},\mu_{21},\mu_{12},\mu_{03}\f$. \param alpha_ref : value of the reference alpha. */ -vpMomentAlpha::vpMomentAlpha(const std::vector &ref_, - double alpha_ref) +vpMomentAlpha::vpMomentAlpha(const std::vector &ref_, double alpha_ref) : vpMoment(), isRef(false), symmetric(false), ref(ref_), alphaRef(alpha_ref) { - for (std::vector::const_iterator it = ref_.begin(); - it != ref_.end(); ++it) + for (std::vector::const_iterator it = ref_.begin(); it != ref_.end(); ++it) if (std::fabs(*it) <= 1e-4) symmetric = true; @@ -83,15 +77,12 @@ void vpMomentAlpha::compute() bool found_moment_centered; const vpMomentCentered &momentCentered = - (static_cast( - getMoments().get("vpMomentCentered", found_moment_centered))); + (static_cast(getMoments().get("vpMomentCentered", found_moment_centered))); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); - double t = 2.0 * momentCentered.get(1, 1) / - (momentCentered.get(2, 0) - momentCentered.get(0, 2)); + double t = 2.0 * momentCentered.get(1, 1) / (momentCentered.get(2, 0) - momentCentered.get(0, 2)); // double alpha = 0.5 * atan2(2.0 * momentCentered.get(1, 1), // (momentCentered.get(2, 0) - momentCentered.get(0, 2))); double alpha = 0.5 * atan(t); @@ -119,11 +110,9 @@ void vpMomentAlpha::compute() double r12_i_k = pow(r12, (int)(i - k)); double comb_i_k = static_cast(vpMath::comb(i, k)); for (unsigned int l = 0; l <= j; l++) { - rotMu[idx] += static_cast( - 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))); + rotMu[idx] += static_cast(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; } @@ -135,11 +124,8 @@ void vpMomentAlpha::compute() double sum = 0.; bool signChange = true; for (unsigned int i = 0; i < 4; i++) { - if (std::fabs(rotMu[i]) > - 1e10 * std::numeric_limits::epsilon() && - std::fabs(ref[i]) > - 1e10 * std::numeric_limits::epsilon() && - rotMu[i] * ref[i] > 0) + if (std::fabs(rotMu[i]) > 1e10 * std::numeric_limits::epsilon() && + std::fabs(ref[i]) > 1e10 * std::numeric_limits::epsilon() && rotMu[i] * ref[i] > 0) signChange = false; sum += std::fabs(rotMu[i] * ref[i]); } @@ -164,8 +150,7 @@ void vpMomentAlpha::compute() 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; + os << "Alpha = " << c.values[0] << "rad = " << vpMath::deg(c.values[0]) << "deg " << std::endl; return os; } @@ -177,11 +162,9 @@ void vpMomentAlpha::printDependencies(std::ostream &os) const os << (__FILE__) << std::endl; bool found_moment_centered; const vpMomentCentered &momentCentered = - (static_cast( - getMoments().get("vpMomentCentered", found_moment_centered))); + (static_cast(getMoments().get("vpMomentCentered", found_moment_centered))); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); os << "mu11 = " << momentCentered.get(1, 1) << "\t"; os << "mu20 = " << momentCentered.get(2, 0) << "\t"; diff --git a/modules/core/src/tracking/moments/vpMomentArea.cpp b/modules/core/src/tracking/moments/vpMomentArea.cpp index 490160aed0..ac962a70d8 100644 --- a/modules/core/src/tracking/moments/vpMomentArea.cpp +++ b/modules/core/src/tracking/moments/vpMomentArea.cpp @@ -57,11 +57,9 @@ void vpMomentArea::compute() * linked to it */ const vpMomentCentered &momentCentered = - static_cast( - getMoments().get("vpMomentCentered", found_moment_centered)); + static_cast(getMoments().get("vpMomentCentered", found_moment_centered)); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); values[0] = momentCentered.get(2, 0) + momentCentered.get(0, 2); } else { values[0] = getObject().get(0, 0); @@ -94,11 +92,9 @@ void vpMomentArea::printDependencies(std::ostream &os) const bool found_moment_centered; const vpMomentCentered &momentCentered = - static_cast( - getMoments().get("vpMomentCentered", found_moment_centered)); + static_cast(getMoments().get("vpMomentCentered", found_moment_centered)); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); if (getObject().getType() == vpMomentObject::DISCRETE) { os << "mu20 = " << momentCentered.get(2, 0) << "\t"; diff --git a/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp b/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp index d815527075..3ccde63b24 100644 --- a/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp +++ b/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp @@ -51,15 +51,13 @@ void vpMomentAreaNormalized::compute() /* 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*/ + .get() is a member function of vpMomentDatabase that returns + a specific moment which is linked to it*/ const vpMomentCentered &momentCentered = - static_cast( - getMoments().get("vpMomentCentered", found_moment_centered)); + static_cast(getMoments().get("vpMomentCentered", found_moment_centered)); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); double a; /* getObject() returns a reference to the vpMomentObject from which @@ -78,8 +76,7 @@ void vpMomentAreaNormalized::compute() converges. \param desired_depth : desired depth \e Z* when the visual servoing converges. */ -vpMomentAreaNormalized::vpMomentAreaNormalized(double desired_surface, - double desired_depth) +vpMomentAreaNormalized::vpMomentAreaNormalized(double desired_surface, double desired_depth) : vpMoment(), desiredSurface(desired_surface), desiredDepth(desired_depth) { values.resize(1); @@ -88,8 +85,7 @@ vpMomentAreaNormalized::vpMomentAreaNormalized(double desired_surface, /*! Outputs the moment's values to a stream. */ -VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentAreaNormalized &m) +VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentAreaNormalized &m) { os << (__FILE__) << std::endl; os << "An = " << m.values[0] << std::endl; @@ -112,11 +108,9 @@ void vpMomentAreaNormalized::printDependencies(std::ostream &os) const bool found_moment_centered; const vpMomentCentered &momentCentered = - static_cast( - getMoments().get("vpMomentCentered", found_moment_centered)); + static_cast(getMoments().get("vpMomentCentered", found_moment_centered)); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); double a; if (getObject().getType() == vpMomentObject::DISCRETE) diff --git a/modules/core/src/tracking/moments/vpMomentBasic.cpp b/modules/core/src/tracking/moments/vpMomentBasic.cpp index aff927a796..7641b0454d 100644 --- a/modules/core/src/tracking/moments/vpMomentBasic.cpp +++ b/modules/core/src/tracking/moments/vpMomentBasic.cpp @@ -49,10 +49,7 @@ vpMomentBasic::vpMomentBasic() : vpMoment() {} Same behaviour as vpMomentObject. */ -const std::vector &vpMomentBasic::get() const -{ - return getObject().get(); -} +const std::vector &vpMomentBasic::get() const { return getObject().get(); } /*! Gets the desired moment using indexes. @@ -62,10 +59,7 @@ const std::vector &vpMomentBasic::get() const Same behaviour as vpMomentObject. */ -double vpMomentBasic::get(unsigned int i, unsigned int j) const -{ - return getObject().get(i, j); -} +double vpMomentBasic::get(unsigned int i, unsigned int j) const { return getObject().get(i, j); } /*! Dummy function. Everything is already done in object. diff --git a/modules/core/src/tracking/moments/vpMomentCInvariant.cpp b/modules/core/src/tracking/moments/vpMomentCInvariant.cpp index 178be2de1c..00ee829cbe 100644 --- a/modules/core/src/tracking/moments/vpMomentCInvariant.cpp +++ b/modules/core/src/tracking/moments/vpMomentCInvariant.cpp @@ -46,8 +46,7 @@ (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) + : 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); } @@ -57,8 +56,7 @@ vpMomentCInvariant::vpMomentCInvariant(bool flg_sxsynormalization) final invariants. \param momentCentered : centered moments \param I : invariant output values */ -void vpMomentCInvariant::computeI(const vpMomentCentered &momentCentered, - std::vector &I_val) +void vpMomentCInvariant::computeI(const vpMomentCentered &momentCentered, std::vector &I_val) { double mu30 = momentCentered.get(3, 0); @@ -120,33 +118,24 @@ void vpMomentCInvariant::computeI(const vpMomentCentered &momentCentered, 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[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[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[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[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; @@ -175,17 +164,12 @@ void vpMomentCInvariant::computeI(const vpMomentCentered &momentCentered, * 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 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::fabs(a)) / - (II[3] * - sqrt(std::fabs( - II[3]))); // d is the normalization factor for 3rd order moments + sqrt(std::fabs(a)) / (II[3] * sqrt(std::fabs(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; @@ -211,17 +195,14 @@ void vpMomentCInvariant::computeI(const vpMomentCentered &momentCentered, void vpMomentCInvariant::compute() { if (getObject().getOrder() < 5) - throw vpException(vpException::notInitialized, - "Order is not high enough for vpMomentCInvariant. " - "Specify at least order 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( - getMoments().get("vpMomentCentered", found_moment_centered))); + (static_cast(getMoments().get("vpMomentCentered", found_moment_centered))); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); computeI(momentCentered, I); double II3_2 = II[3] * II[3]; @@ -285,10 +266,7 @@ void vpMomentCInvariant::calcSxSyNormalized(double &sx, double &sy) const 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; -} +void vpMomentCInvariant::printI(unsigned int index) { std::cout << "I(" << index << ")=" << I[index] << std::endl; } /*! Print out all invariants that were computed @@ -297,9 +275,8 @@ void vpMomentCInvariant::printI(unsigned int index) */ 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 + 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; @@ -308,8 +285,7 @@ void vpMomentCInvariant::printInvariants(std::ostream &os) const /*! Outputs the moment's values to a stream. */ -VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentCInvariant &c) +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; diff --git a/modules/core/src/tracking/moments/vpMomentCentered.cpp b/modules/core/src/tracking/moments/vpMomentCentered.cpp index d1d4fc5e7a..5902580c90 100644 --- a/modules/core/src/tracking/moments/vpMomentCentered.cpp +++ b/modules/core/src/tracking/moments/vpMomentCentered.cpp @@ -68,11 +68,9 @@ void vpMomentCentered::compute() values.resize((getObject().getOrder() + 1) * (getObject().getOrder() + 1)); const vpMomentGravityCenter &momentGravity = - static_cast( - getMoments().get("vpMomentGravityCenter", found_moment_gravity)); + static_cast(getMoments().get("vpMomentGravityCenter", found_moment_gravity)); if (!found_moment_gravity) - throw vpException(vpException::notInitialized, - "vpMomentGravityCenter not found"); + throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found"); unsigned int order = getObject().getOrder() + 1; for (unsigned int j = 0; j < (order); j++) { @@ -83,10 +81,8 @@ void vpMomentCentered::compute() double Xg_i_k = pow(-momentGravity.get()[0], (int)(i - k)); double comb_i_k = static_cast(vpMath::comb(i, k)); for (unsigned int l = 0; l <= j; l++) { - values[c] += - static_cast(comb_i_k * vpMath::comb(j, l) * Xg_i_k * - pow(-momentGravity.get()[1], (int)(j - l)) * - getObject().get(k, l)); + values[c] += static_cast(comb_i_k * vpMath::comb(j, l) * Xg_i_k * + pow(-momentGravity.get()[1], (int)(j - l)) * getObject().get(k, l)); } } } @@ -134,15 +130,13 @@ u30 x x x 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) +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) + if ((i % (m.getObject().getOrder() + 1) + i / (m.getObject().getOrder() + 1)) < m.getObject().getOrder() + 1) os << m.values[i]; else os << "x"; @@ -189,11 +183,9 @@ void vpMomentCentered::printDependencies(std::ostream &os) const */ bool found_moment_gravity; const vpMomentGravityCenter &momentGravity = - static_cast( - getMoments().get("vpMomentGravityCenter", found_moment_gravity)); + static_cast(getMoments().get("vpMomentGravityCenter", found_moment_gravity)); if (!found_moment_gravity) - throw vpException(vpException::notInitialized, - "vpMomentGravityCenter not found"); + 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 index da11774b14..fc39601484 100644 --- a/modules/core/src/tracking/moments/vpMomentCommon.cpp +++ b/modules/core/src/tracking/moments/vpMomentCommon.cpp @@ -51,13 +51,10 @@ flg_sxsyfromnormalized : flag to enable calculation of sx,sy from normalized moments. */ -vpMomentCommon::vpMomentCommon(double dstSurface, - const std::vector &ref, - double refAlpha, double dstZ, +vpMomentCommon::vpMomentCommon(double dstSurface, const std::vector &ref, double refAlpha, double dstZ, bool flg_sxsyfromnormalized) - : vpMomentDatabase(), momentBasic(), momentGravity(), momentCentered(), - momentGravityNormalized(), momentSurfaceNormalized(dstSurface, dstZ), - momentCInvariant(), momentAlpha(ref, refAlpha), momentArea() + : vpMomentDatabase(), momentBasic(), momentGravity(), momentCentered(), momentGravityNormalized(), + momentSurfaceNormalized(dstSurface, dstZ), momentCInvariant(), momentAlpha(ref, refAlpha), momentArea() { momentCInvariant = new vpMomentCInvariant(flg_sxsyfromnormalized); @@ -90,36 +87,36 @@ int main() { // Define two discrete points vpPoint p; - std::vector 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); + std::vector 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 + 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.); + 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(db.get("vpMomentCInvariant",success)); - if(success){ - std::cout << C.get(0) << std:: std::endl; - }else - std::cout << "vpMomentCInvariant not found." << std::endl; + vpMomentCInvariant& C = static_cast(db.get("vpMomentCInvariant",success)); + if(success) { + std::cout << C.get(0) << std:: std::endl; + } else + std::cout << "vpMomentCInvariant not found." << std::endl; return 0; } diff --git a/modules/core/src/tracking/moments/vpMomentDatabase.cpp b/modules/core/src/tracking/moments/vpMomentDatabase.cpp index a16532637f..524ea8c3d5 100644 --- a/modules/core/src/tracking/moments/vpMomentDatabase.cpp +++ b/modules/core/src/tracking/moments/vpMomentDatabase.cpp @@ -52,8 +52,7 @@ */ void vpMomentDatabase::add(vpMoment &moment, const char *name) { - moments.insert( - std::pair((const char *)name, &moment)); + moments.insert(std::pair((const char *)name, &moment)); } /*! @@ -64,8 +63,7 @@ void vpMomentDatabase::add(vpMoment &moment, const char *name) */ const vpMoment &vpMomentDatabase::get(const char *type, bool &found) const { - std::map::const_iterator it = moments.find(type); + std::map::const_iterator it = moments.find(type); found = (it != moments.end()); return *(it->second); @@ -83,8 +81,7 @@ const vpMoment &vpMomentDatabase::get(const char *type, bool &found) const */ void vpMomentDatabase::updateAll(vpMomentObject &object) { - std::map::const_iterator itr; + std::map::const_iterator itr; for (itr = moments.begin(); itr != moments.end(); ++itr) { (*itr).second->update(object); } @@ -93,11 +90,9 @@ void vpMomentDatabase::updateAll(vpMomentObject &object) /*! Outputs all the moments values in the database to a stream. */ -VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentDatabase &m) +VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentDatabase &m) { - std::map::const_iterator itr; + std::map::const_iterator itr; os << "{"; for (itr = m.moments.begin(); itr != m.moments.end(); ++itr) { diff --git a/modules/core/src/tracking/moments/vpMomentGravityCenter.cpp b/modules/core/src/tracking/moments/vpMomentGravityCenter.cpp index 182874858e..5a221eea01 100644 --- a/modules/core/src/tracking/moments/vpMomentGravityCenter.cpp +++ b/modules/core/src/tracking/moments/vpMomentGravityCenter.cpp @@ -51,29 +51,21 @@ void vpMomentGravityCenter::compute() /*! Default constructor. */ -vpMomentGravityCenter::vpMomentGravityCenter() : vpMoment() -{ - values.resize(2); -} +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 &vpMomentGravityCenter::get() const -{ - return values; -} +const std::vector &vpMomentGravityCenter::get() const { return values; } /*! Outputs the moment's values to a stream. */ -VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentGravityCenter &m) +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; + os << "(Xg,Yg) = (" << m.values[0] << ", " << m.values[1] << ")" << std::endl; return os; } diff --git a/modules/core/src/tracking/moments/vpMomentGravityCenterNormalized.cpp b/modules/core/src/tracking/moments/vpMomentGravityCenterNormalized.cpp index 7aeec3bb04..bc7dbe2ded 100644 --- a/modules/core/src/tracking/moments/vpMomentGravityCenterNormalized.cpp +++ b/modules/core/src/tracking/moments/vpMomentGravityCenterNormalized.cpp @@ -51,19 +51,15 @@ void vpMomentGravityCenterNormalized::compute() bool found_moment_gravity; bool found_moment_surface_normalized; - const vpMomentAreaNormalized &momentSurfaceNormalized = - static_cast(getMoments().get( - "vpMomentAreaNormalized", found_moment_surface_normalized)); + const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast( + getMoments().get("vpMomentAreaNormalized", found_moment_surface_normalized)); const vpMomentGravityCenter &momentGravity = - static_cast( - getMoments().get("vpMomentGravityCenter", found_moment_gravity)); + static_cast(getMoments().get("vpMomentGravityCenter", found_moment_gravity)); if (!found_moment_surface_normalized) - throw vpException(vpException::notInitialized, - "vpMomentAreaNormalized not found"); + throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found"); if (!found_moment_gravity) - throw vpException(vpException::notInitialized, - "vpMomentGravityCenter not found"); + throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found"); double Xn = momentGravity.get()[0] * momentSurfaceNormalized.get()[0]; double Yn = momentGravity.get()[1] * momentSurfaceNormalized.get()[0]; @@ -75,20 +71,15 @@ void vpMomentGravityCenterNormalized::compute() /*! Default constructor. */ -vpMomentGravityCenterNormalized::vpMomentGravityCenterNormalized() - : vpMomentGravityCenter() -{ -} +vpMomentGravityCenterNormalized::vpMomentGravityCenterNormalized() : vpMomentGravityCenter() {} /*! Outputs the moment's values to a stream. */ -VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentGravityCenterNormalized &m) +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; + os << "(Xn,Yn) = (" << m.values[0] << ", " << m.values[1] << ")" << std::endl; return os; } @@ -97,26 +88,21 @@ Prints the dependent moments, 1. centre of gravity 2. normalized area moment */ -void vpMomentGravityCenterNormalized::printDependencies( - std::ostream &os) const +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(getMoments().get( - "vpMomentAreaNormalized", found_moment_surface_normalized)); + const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast( + getMoments().get("vpMomentAreaNormalized", found_moment_surface_normalized)); const vpMomentGravityCenter &momentGravity = - static_cast( - getMoments().get("vpMomentGravityCenter", found_moment_gravity)); + static_cast(getMoments().get("vpMomentGravityCenter", found_moment_gravity)); if (!found_moment_surface_normalized) - throw vpException(vpException::notInitialized, - "vpMomentAreaNormalized not found"); + throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found"); if (!found_moment_gravity) - throw vpException(vpException::notInitialized, - "vpMomentGravityCenter not found"); + 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 index 8fa5a9dedd..f0f710f011 100644 --- a/modules/core/src/tracking/moments/vpMomentObject.cpp +++ b/modules/core/src/tracking/moments/vpMomentObject.cpp @@ -60,8 +60,7 @@ \return moment value */ -double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, - const std::vector &points) +double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, const std::vector &points) { unsigned int i, k, l; double den, mom; @@ -69,8 +68,7 @@ double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, double y_l; double y_q_l; - den = - static_cast((p + q + 2) * (p + q + 1) * vpMath::comb(p + q, p)); + den = static_cast((p + q + 2) * (p + q + 1) * vpMath::comb(p + q, p)); mom = 0.0; for (i = 1; i <= points.size() - 1; i++) { @@ -82,17 +80,15 @@ double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, for (l = 0; l <= q; l++) { y_q_l = pow(points[i - 1].get_y(), (int)(q - l)); - s += static_cast(vpMath::comb(k + l, l) * - vpMath::comb(p + q - k - l, q - l) * x_k * - x_p_k * y_l * y_q_l); + s += static_cast(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())); + s *= ((points[i - 1].get_x()) * (points[i].get_y()) - (points[i].get_x()) * (points[i - 1].get_y())); mom += s; } mom /= den; @@ -103,13 +99,16 @@ double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, 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 + 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 &cache, double x, - double y) +void vpMomentObject::cacheValues(std::vector &cache, double x, double y) { cache[0] = 1; @@ -130,15 +129,13 @@ void vpMomentObject::cacheValues(std::vector &cache, double x, * Manikandan.B * Need to cache intensity along with the coordinates for photometric moments */ -void vpMomentObject::cacheValues(std::vector &cache, double x, - double y, double IntensityNormalized) +void vpMomentObject::cacheValues(std::vector &cache, double x, double y, double IntensityNormalized) { cache[0] = IntensityNormalized; double invIntensityNormalized = 0.; - if (std::fabs(IntensityNormalized) >= - std::numeric_limits::epsilon()) + if (std::fabs(IntensityNormalized) >= std::numeric_limits::epsilon()) invIntensityNormalized = 1.0 / IntensityNormalized; for (unsigned int i = 1; i < order; i++) @@ -149,8 +146,7 @@ void vpMomentObject::cacheValues(std::vector &cache, double x, for (unsigned int j = 1; j < order; j++) { for (unsigned int i = 1; i < order - j; i++) { - cache[j * order + i] = - cache[j * order] * cache[i] * invIntensityNormalized; + cache[j * order + i] = cache[j * order] * cache[i] * invIntensityNormalized; } } } @@ -178,25 +174,26 @@ int main() { // Define the contour of an object by a 5 clockwise vertices on a plane vpPoint p; - std::vector vec_p; // vector that contains the vertices of the -contour polygon + std::vector 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) + 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); - 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 + 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 @@ -212,30 +209,29 @@ int main() vpPoint p; std::vector 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) + 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); - 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 + 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 &points) { if (type == vpMomentObject::DENSE_POLYGON) { - if (std::fabs(points.rbegin()->get_x() - points.begin()->get_x()) > - std::numeric_limits::epsilon() || - std::fabs(points.rbegin()->get_y() - points.begin()->get_y()) > - std::numeric_limits::epsilon()) { + if (std::fabs(points.rbegin()->get_x() - points.begin()->get_x()) > std::numeric_limits::epsilon() || + std::fabs(points.rbegin()->get_y() - points.begin()->get_y()) > std::numeric_limits::epsilon()) { points.resize(points.size() + 1); points[points.size() - 1] = points[0]; } @@ -290,8 +286,7 @@ image \endcode */ -void vpMomentObject::fromImage(const vpImage &image, - unsigned char threshold, +void vpMomentObject::fromImage(const vpImage &image, unsigned char threshold, const vpCameraParameters &cam) { #ifdef VISP_HAVE_OPENMP @@ -358,8 +353,7 @@ void vpMomentObject::fromImage(const vpImage &image, // Normalisation equivalent to sampling interval/pixel size delX x delY double norm_factor = 1. / (cam.get_px() * cam.get_py()); - for (std::vector::iterator it = values.begin(); it != values.end(); - ++it) { + for (std::vector::iterator it = values.begin(); it != values.end(); ++it) { *it = (*it) * norm_factor; } } @@ -376,10 +370,8 @@ void vpMomentObject::fromImage(const vpImage &image, * calculation are normalized w.r.t pixel size available from camera * parameters */ -void vpMomentObject::fromImage(const vpImage &image, - const vpCameraParameters &cam, - vpCameraImgBckGrndType bg_type, - bool normalize_with_pix_size) +void vpMomentObject::fromImage(const vpImage &image, const vpCameraParameters &cam, + vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size) { std::vector cache(order * order, 0.); values.assign(order * order, 0); @@ -398,8 +390,7 @@ void vpMomentObject::fromImage(const vpImage &image, double iscale = 1.0; if (flg_normalize_intensity) { // This makes the image a probability density // function - double Imax = - 255.; // To check the effect of gray level change. ISR Coimbra + double Imax = 255.; // To check the effect of gray level change. ISR Coimbra iscale = 1.0 / Imax; } @@ -437,9 +428,8 @@ void vpMomentObject::fromImage(const vpImage &image, 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) + 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 (unsigned int k = 0; k < order; k++) { @@ -456,8 +446,7 @@ void vpMomentObject::fromImage(const vpImage &image, 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::iterator it = values.begin(); - it != values.end(); ++it) { + for (std::vector::iterator it = values.begin(); it != values.end(); ++it) { *it = (*it) * norm_factor; } } @@ -471,8 +460,7 @@ 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 + flg_normalize_intensity = true; // By default, the intensity values are normalized values.resize((order + 1) * (order + 1)); values.assign((order + 1) * (order + 1), 0); } @@ -505,8 +493,7 @@ void vpMomentObject::init(const vpMomentObject &objin) 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() + : flg_normalize_intensity(true), order(max_order + 1), type(vpMomentObject::DENSE_FULL_OBJECT), values() { init(max_order); } @@ -515,8 +502,7 @@ vpMomentObject::vpMomentObject(unsigned int max_order) Copy constructor */ vpMomentObject::vpMomentObject(const vpMomentObject &srcobj) - : flg_normalize_intensity(true), order(1), - type(vpMomentObject::DENSE_FULL_OBJECT), values() + : flg_normalize_intensity(true), order(1), type(vpMomentObject::DENSE_FULL_OBJECT), values() { init(srcobj); } @@ -534,7 +520,9 @@ 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); +code: +\code +vpMomentObject obj(3); // ... initialise the object using fromVector() or fromImage() std::vector mij = obj.get(); double m12; @@ -566,14 +554,12 @@ double vpMomentObject::get(unsigned int i, unsigned int j) const \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) +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."); + 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; } @@ -593,8 +579,7 @@ void vpMomentObject::set(unsigned int i, unsigned int j, \endcode */ -VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpMomentObject &m) +VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentObject &m) { for (unsigned int i = 0; i < m.values.size(); i++) { @@ -617,17 +602,14 @@ VISP_EXPORT std::ostream &operator<<(std::ostream &os, 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) +void vpMomentObject::printWithIndices(const vpMomentObject &momobj, std::ostream &os) { std::vector moment = momobj.get(); - os << std::endl - << "Order of vpMomentObject: " << momobj.getOrder() << std::endl; + 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 << "m[" << l << "," << k << "] = " << moment[k * (momobj.getOrder() + 1) + l] << "\t"; } os << std::endl; } @@ -639,13 +621,14 @@ void vpMomentObject::printWithIndices(const vpMomentObject &momobj, \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); + 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 +file or std::cout. For instance, like +\code // Print to console Mpq.maplePrint(std::cout); // Or write to a file @@ -677,8 +660,9 @@ vpMatrix vpMomentObject::convertTovpMatrix(const vpMomentObject &momobj) 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* obj = new vpWeightedMomentObject(weightfunc,ORDER); + \endcode + where vpWeightedMomentObject is child class of vpMomentObject */ vpMomentObject::~vpMomentObject() { diff --git a/modules/core/src/tracking/vpTracker.cpp b/modules/core/src/tracking/vpTracker.cpp index b571b82c2f..73ee14a73b 100644 --- a/modules/core/src/tracking/vpTracker.cpp +++ b/modules/core/src/tracking/vpTracker.cpp @@ -48,10 +48,7 @@ void vpTracker::init() { cPAvailable = false; } vpTracker::vpTracker() : p(), cP(), cPAvailable(false) {} -vpTracker::vpTracker(const vpTracker &tracker) : p(), cP(), cPAvailable(false) -{ - *this = tracker; -} +vpTracker::vpTracker(const vpTracker &tracker) : p(), cP(), cPAvailable(false) { *this = tracker; } vpTracker &vpTracker::operator=(const vpTracker &tracker) { diff --git a/modules/core/test/camera/testCameraParametersConversion.cpp b/modules/core/test/camera/testCameraParametersConversion.cpp index d75a6ad083..2e60bcedf2 100644 --- a/modules/core/test/camera/testCameraParametersConversion.cpp +++ b/modules/core/test/camera/testCameraParametersConversion.cpp @@ -71,8 +71,7 @@ int main() kdu_dist = 0.1771165148; cam.initPersProjWithoutDistortion(px, py, u0, v0); - camDist.initPersProjWithDistortion(px_dist, py_dist, u0_dist, v0_dist, - kud_dist, kdu_dist); + camDist.initPersProjWithDistortion(px_dist, py_dist, u0_dist, v0_dist, kud_dist, kdu_dist); double u1 = 320; double v1 = 240; diff --git a/modules/core/test/image/testConversion.cpp b/modules/core/test/image/testConversion.cpp index f3333055ed..92401b8047 100644 --- a/modules/core/test/image/testConversion.cpp +++ b/modules/core/test/image/testConversion.cpp @@ -68,8 +68,7 @@ \param nbiter : Iteration number. */ -void usage(const char *name, const char *badparam, std::string ipath, - std::string opath, std::string user, int nbiter) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user, int nbiter) { fprintf(stdout, "\n\ Test image conversions.\n\ @@ -119,8 +118,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, const std::string &user, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, const std::string &user, int &nbIterations) { const char *optarg_; @@ -164,40 +162,33 @@ bool getOptions(int argc, const char **argv, std::string &ipath, return true; } -void computeRegularRGBaToGrayscale(const unsigned char *rgba, - unsigned char *grey, unsigned int size) +void computeRegularRGBaToGrayscale(const unsigned char *rgba, unsigned char *grey, unsigned int size) { const unsigned char *pt_input = rgba; const 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_output = (unsigned char)(0.2126 * (*pt_input) + 0.7152 * (*(pt_input + 1)) + 0.0722 * (*(pt_input + 2))); pt_input += 4; pt_output++; } } -void computeRegularRGBToGrayscale(const unsigned char *rgb, - unsigned char *grey, unsigned int size) +void computeRegularRGBToGrayscale(const unsigned char *rgb, unsigned char *grey, unsigned int size) { const unsigned char *pt_input = rgb; const 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_output = (unsigned char)(0.2126 * (*pt_input) + 0.7152 * (*(pt_input + 1)) + 0.0722 * (*(pt_input + 2))); pt_input += 3; pt_output++; } } -void computeRegularBGRToGrayscale(unsigned char *bgr, unsigned char *grey, - unsigned int width, unsigned int height, +void computeRegularBGRToGrayscale(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 @@ -213,8 +204,7 @@ void computeRegularBGRToGrayscale(unsigned char *bgr, unsigned char *grey, for (i = 0; i < height; i++) { unsigned char *line = src; for (j = 0; j < width; j++) { - *grey++ = (unsigned char)(0.2126 * *(line + 2) + 0.7152 * *(line + 1) + - 0.0722 * *(line + 0)); + *grey++ = (unsigned char)(0.2126 * *(line + 2) + 0.7152 * *(line + 1) + 0.0722 * *(line + 0)); line += 3; } @@ -224,16 +214,14 @@ void computeRegularBGRToGrayscale(unsigned char *bgr, unsigned char *grey, } #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) -void computeRegularBGRToGrayscale(const cv::Mat &src, - vpImage &dest) +void computeRegularBGRToGrayscale(const cv::Mat &src, vpImage &dest) { if (src.type() == CV_8UC3) { dest.resize((unsigned int)src.rows, (unsigned int)src.cols); if (src.isContinuous()) { - computeRegularBGRToGrayscale( - (unsigned char *)src.data, (unsigned char *)dest.bitmap, - (unsigned int)src.cols, (unsigned int)src.rows, false); + computeRegularBGRToGrayscale((unsigned char *)src.data, (unsigned char *)dest.bitmap, (unsigned int)src.cols, + (unsigned int)src.rows, false); } } } @@ -270,8 +258,7 @@ int main(int argc, const char **argv) vpIoTools::getUserName(username); // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_opath, username, - nbIterations) == false) { + if (getOptions(argc, argv, opt_ipath, opt_opath, username, nbIterations) == false) { exit(-1); } @@ -293,8 +280,7 @@ int main(int argc, const char **argv) usage(argv[0], NULL, ipath, opt_opath, username, nbIterations); std::cerr << std::endl << "ERROR:" << std::endl; std::cerr << " Cannot create " << opath << std::endl; - std::cerr << " Check your -o " << opt_opath << " option " - << std::endl; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; exit(-1); } } @@ -305,8 +291,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -315,13 +300,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, opt_opath, username, nbIterations); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -333,8 +315,7 @@ int main(int argc, const char **argv) vpImage Ic; // Color image //-------------------- .pgm -> .ppm - std::cout << "** Convert a grey image (.pgm) to a color image (.ppm)" - << std::endl; + std::cout << "** Convert a grey image (.pgm) to a color image (.ppm)" << std::endl; // Load a grey image from the disk filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm"); std::cout << " Load " << filename << std::endl; @@ -346,8 +327,7 @@ int main(int argc, const char **argv) vpImageIo::write(Ic, filename); //-------------------- .ppm -> .pgm - std::cout << "** Convert a color image (.ppm) to a grey image (.pgm)" - << std::endl; + std::cout << "** Convert a color image (.ppm) to a grey image (.pgm)" << std::endl; // Load a color image from the disk filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm"); std::cout << " Load " << filename << std::endl; @@ -365,9 +345,8 @@ int main(int argc, const char **argv) // Convert a YUV pixel value to a RGB value vpImageConvert::YUVToRGB(y, u, v, r, g, b); - std::cout << " y(" << (int)y << ") u(" << (int)u << ") v(" << (int)v - << ") = r(" << (int)r << ") g(" << (int)g << ") b(" << (int)b - << ")" << std::endl; + std::cout << " y(" << (int)y << ") u(" << (int)u << ") v(" << (int)v << ") = r(" << (int)r << ") g(" << (int)g + << ") b(" << (int)b << ")" << std::endl; #ifdef VISP_HAVE_OPENCV #if VISP_HAVE_OPENCV_VERSION < 0x020408 @@ -381,10 +360,8 @@ int main(int argc, const char **argv) /* Read the color image */ - std::cout << " Reading the color image with opencv: " << filename - << std::endl; - if ((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR)) == - NULL) { + std::cout << " Reading the color image with opencv: " << filename << std::endl; + if ((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR)) == NULL) { std::cout << " Cannot read image: " << filename << std::endl; return (-1); } @@ -398,12 +375,10 @@ int main(int argc, const char **argv) filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm"); /* Read the pgm image */ - std::cout << " Reading the greyscale image with opencv: " << filename - << std::endl; + std::cout << " Reading the greyscale image with opencv: " << filename << std::endl; if (image != NULL) cvReleaseImage(&image); - if ((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE)) == - NULL) { + if ((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE)) == NULL) { std::cout << " Cannot read image: " << filename << std::endl; return (-1); } @@ -417,18 +392,15 @@ int main(int argc, const char **argv) /////////////////////////// // Convert a IplImage to a vpImage //////////////////////////// - std::cout << "** Convert an IplImage to a vpImage" - << std::endl; + std::cout << "** Convert an IplImage to a vpImage" << std::endl; filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm"); /* Read the color image */ - std::cout << " Reading the color image with opencv: " << filename - << std::endl; + std::cout << " Reading the color image with opencv: " << filename << std::endl; if (image != NULL) cvReleaseImage(&image); - if ((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR)) == - NULL) { + if ((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR)) == NULL) { std::cout << " Cannot read image: " << filename << std::endl; return (-1); } @@ -443,12 +415,10 @@ int main(int argc, const char **argv) /* Read the pgm image */ - std::cout << " Reading the greyscale image with opencv: " << filename - << std::endl; + std::cout << " Reading the greyscale image with opencv: " << filename << std::endl; if (image != NULL) cvReleaseImage(&image); - if ((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE)) == - NULL) { + if ((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE)) == NULL) { std::cout << " Cannot read image: " << filename << std::endl; return (-1); } @@ -485,8 +455,7 @@ int main(int argc, const char **argv) //////////////////////////////////////// // Convert a vpImage to an IplImage //////////////////////////////////////// - std::cout << "** Convert a vpImage to an IplImage" - << std::endl; + std::cout << "** Convert a vpImage to an IplImage" << std::endl; filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm"); /* Read the grey image */ @@ -500,8 +469,7 @@ int main(int argc, const char **argv) std::cout << " Write " << filename << std::endl; if ((cvSaveImage(filename.c_str(), image)) == 0) { - std::cout << " Cannot write image: " << std::endl - << filename << std::endl; + std::cout << " Cannot write image: " << std::endl << filename << std::endl; if (image != NULL) cvReleaseImage(&image); return (-1); @@ -511,8 +479,7 @@ int main(int argc, const char **argv) if (image != NULL) cvReleaseImage(&image); double t1 = vpTime::measureTimeMs(); - std::cout << "== Conversion c interface : " << t1 - t0 << " ms" - << std::endl; + std::cout << "== Conversion c interface : " << t1 - t0 << " ms" << std::endl; #endif /* ------------------------------------------------------------------------ */ @@ -527,10 +494,8 @@ int main(int argc, const char **argv) std::cout << "** Convert a cv::Mat to a vpImage" << std::endl; cv::Mat imageMat; filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm"); - std::cout << " Reading the color image with c++ interface of opencv: " - << filename << std::endl; - imageMat = - cv::imread(filename, 1); // force to a three channel color image. + std::cout << " Reading the color image with c++ interface of opencv: " << filename << std::endl; + imageMat = cv::imread(filename, 1); // force to a three channel color image. if (imageMat.data == NULL) { std::cout << " Cannot read image: " << filename << std::endl; return -1; @@ -543,8 +508,7 @@ int main(int argc, const char **argv) filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm"); /* Read the pgm image */ - std::cout << " Reading the greyscale image with opencv: " << filename - << std::endl; + std::cout << " Reading the greyscale image with opencv: " << filename << std::endl; imageMat = cv::imread(filename, 0); // forced to grayscale. if (imageMat.data == NULL) { std::cout << " Cannot read image: " << filename << std::endl; @@ -558,16 +522,13 @@ int main(int argc, const char **argv) /////////////////////////// // Convert a cv::Mat to a vpImage //////////////////////////// - std::cout << "** Convert a cv::Mat to a vpImage" - << std::endl; + std::cout << "** Convert a cv::Mat to a vpImage" << std::endl; filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm"); /* Read the color image */ - std::cout << " Reading the color image with opencv: " << filename - << std::endl; - imageMat = - cv::imread(filename, 1); // force to a three channel color image. + std::cout << " Reading the color image with opencv: " << filename << std::endl; + imageMat = cv::imread(filename, 1); // force to a three channel color image. if (imageMat.data == NULL) { std::cout << " Cannot read image: " << filename << std::endl; return -1; @@ -581,8 +542,7 @@ int main(int argc, const char **argv) /* Read the pgm image */ - std::cout << " Reading the greyscale image with opencv: " << filename - << std::endl; + std::cout << " Reading the greyscale image with opencv: " << filename << std::endl; imageMat = cv::imread(filename, 0); if (imageMat.data == NULL) { std::cout << " Cannot read image: " << filename << std::endl; @@ -619,8 +579,7 @@ int main(int argc, const char **argv) //////////////////////////////////////// // Convert a vpImage to a cv::Mat //////////////////////////////////////// - std::cout << "** Convert a vpImage to a cv::Mat" - << std::endl; + std::cout << "** Convert a vpImage to a cv::Mat" << std::endl; filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm"); /* Read the grey image */ @@ -639,16 +598,14 @@ int main(int argc, const char **argv) } std::cout << " Convert result in " << filename << std::endl; double t3 = vpTime::measureTimeMs(); - std::cout << "== Conversion c++ interface : " << t3 - t2 << " ms" - << std::endl; + std::cout << "== Conversion c++ interface : " << t3 - t2 << " ms" << std::endl; #endif #endif //////////////////////////////////// // Split a vpImage to vpImage //////////////////////////////////// - std::cout << "** Split a vpImage to vpImage" - << std::endl; + std::cout << "** Split a vpImage to vpImage" << std::endl; filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm"); /* Read the color image */ @@ -664,8 +621,7 @@ int main(int argc, const char **argv) } double endtime = vpTime::measureTimeMs(); - std::cout << " Time for 1000 split (ms): " << endtime - begintime - << std::endl; + std::cout << " Time for 1000 split (ms): " << endtime - begintime << std::endl; filename = vpIoTools::createFilePath(opath, "Klimt_RChannel.pgm"); /* Save the the current image */ @@ -680,8 +636,7 @@ int main(int argc, const char **argv) //////////////////////////////////// // Merge 4 vpImage (RGBa) to vpImage //////////////////////////////////// - std::cout << "** Merge 4 vpImage (RGBa) to vpImage" - << std::endl; + std::cout << "** Merge 4 vpImage (RGBa) to vpImage" << std::endl; vpImageConvert::split(Ic, &R, &G, &B, &a); begintime = vpTime::measureTimeMs(); vpImage I_merge; @@ -690,8 +645,7 @@ int main(int argc, const char **argv) } endtime = vpTime::measureTimeMs(); - std::cout << " Time for 1000 merge (ms): " << endtime - begintime - << std::endl; + std::cout << " Time for 1000 merge (ms): " << endtime - begintime << std::endl; filename = vpIoTools::createFilePath(opath, "Klimt_merge.ppm"); std::cout << " Resulting image saved in: " << filename << std::endl; @@ -710,8 +664,7 @@ int main(int argc, const char **argv) unsigned char *saturation = new unsigned char[size]; unsigned char *value = new unsigned char[size]; - vpImageConvert::RGBaToHSV((unsigned char *)Ic.bitmap, hue, saturation, - value, size); + vpImageConvert::RGBaToHSV((unsigned char *)Ic.bitmap, hue, saturation, value, size); vpImage I_hue(hue, h, w); vpImage I_saturation(saturation, h, w); vpImage I_value(value, h, w); @@ -726,8 +679,7 @@ int main(int argc, const char **argv) 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); + 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); @@ -754,20 +706,14 @@ int main(int argc, const char **argv) 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) { + 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(Ic[i][j].R) - << " ; I_HSV2RGBa[i][j].R=" - << static_cast(I_HSV2RGBa[i][j].R) << std::endl; + << " ; I_HSV2RGBa[i][j].R=" << static_cast(I_HSV2RGBa[i][j].R) << std::endl; std::cerr << "Ic[i][j].G=" << static_cast(Ic[i][j].G) - << " ; I_HSV2RGBa[i][j].G=" - << static_cast(I_HSV2RGBa[i][j].G) << std::endl; + << " ; I_HSV2RGBa[i][j].G=" << static_cast(I_HSV2RGBa[i][j].G) << std::endl; std::cerr << "Ic[i][j].B=" << static_cast(Ic[i][j].B) - << " ; I_HSV2RGBa[i][j].B=" - << static_cast(I_HSV2RGBa[i][j].B) << std::endl; - throw vpException(vpException::fatalError, - "Problem with conversion between RGB <==> HSV"); + << " ; I_HSV2RGBa[i][j].B=" << static_cast(I_HSV2RGBa[i][j].B) << std::endl; + throw vpException(vpException::fatalError, "Problem with conversion between RGB <==> HSV"); } } } @@ -775,9 +721,7 @@ int main(int argc, const char **argv) //////////////////////////////////// // Test construction of a vpImage from an array with copyData==true //////////////////////////////////// - std::cout - << "** Construction of a vpImage from an array with copyData==true" - << std::endl; + std::cout << "** Construction of a vpImage from an array with copyData==true" << std::endl; unsigned char *rgba2 = new unsigned char[size * 4]; memset(rgba2, 127, size * 4); vpImage I_copyData((vpRGBa *)rgba2, h, w, true); @@ -803,10 +747,8 @@ int main(int argc, const char **argv) filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm"); vpImageIo::read(I_color, filename); - vpImage I_gray_sse(I_color.getHeight(), - I_color.getWidth()); - vpImage I_gray_regular(I_color.getHeight(), - I_color.getWidth()); + vpImage I_gray_sse(I_color.getHeight(), I_color.getWidth()); + vpImage I_gray_regular(I_color.getHeight(), I_color.getWidth()); unsigned char value_sse = 0, value_regular = 0; double t_sse = vpTime::measureTimeMs(); @@ -818,9 +760,7 @@ int main(int argc, const char **argv) double t_regular = vpTime::measureTimeMs(); for (int iteration = 0; iteration < nbIterations; iteration++) { - computeRegularRGBaToGrayscale((unsigned char *)I_color.bitmap, - I_gray_regular.bitmap, - I_color.getSize()); + computeRegularRGBaToGrayscale((unsigned char *)I_color.bitmap, I_gray_regular.bitmap, I_color.getSize()); value_regular += I_gray_regular[0][0]; } t_regular = vpTime::measureTimeMs() - t_regular; @@ -829,24 +769,20 @@ int main(int argc, const char **argv) double rmse_error = 0.0; for (unsigned int i = 0; i < I_color.getHeight(); i++) { for (unsigned int j = 0; j < I_color.getWidth(); j++) { - rmse_error += (I_gray_sse[i][j] - I_gray_regular[i][j]) * - (I_gray_sse[i][j] - I_gray_regular[i][j]); + rmse_error += (I_gray_sse[i][j] - I_gray_regular[i][j]) * (I_gray_sse[i][j] - I_gray_regular[i][j]); } } std::cout << "\n RGBa to Grayscale" << std::endl; - std::cout << " t_regular (" << nbIterations - << " iterations)=" << t_regular << " ms" - << " ; t_sse (" << nbIterations << " iterations)=" << t_sse - << " ms" << std::endl; + std::cout << " t_regular (" << nbIterations << " iterations)=" << t_regular << " ms" + << " ; t_sse (" << nbIterations << " iterations)=" << t_sse << " ms" << std::endl; std::cout << " Speed-up=" << (t_regular / t_sse) << "X" << std::endl; - std::cout << " RMSE error between SSE and regular version: " - << (std::sqrt(rmse_error / I_color.getSize())) << std::endl; + std::cout << " RMSE error between SSE and regular version: " << (std::sqrt(rmse_error / I_color.getSize())) + << std::endl; // To prevent the iteration loop to not be optimized? std::cout << " value_sse=" << static_cast(value_sse) - << " ; value_regular=" << static_cast(value_regular) - << std::endl; + << " ; value_regular=" << static_cast(value_regular) << std::endl; filename = vpIoTools::createFilePath(opath, "I_rgba2gray_sse.pgm"); std::cout << " Resulting image saved in: " << filename << std::endl; @@ -858,61 +794,50 @@ int main(int argc, const char **argv) // RGB to Grayscale conversion unsigned char *rgb_array = new unsigned char[I_color.getSize() * 3]; - vpImageConvert::RGBaToRGB((unsigned char *)I_color.bitmap, rgb_array, - I_color.getSize()); + vpImageConvert::RGBaToRGB((unsigned char *)I_color.bitmap, rgb_array, I_color.getSize()); value_sse = 0; value_regular = 0; - unsigned char *rgb2gray_array_sse = - new unsigned char[I_color.getSize()]; + unsigned char *rgb2gray_array_sse = new unsigned char[I_color.getSize()]; t_sse = vpTime::measureTimeMs(); for (int iteration = 0; iteration < nbIterations; iteration++) { - vpImageConvert::RGBToGrey(rgb_array, rgb2gray_array_sse, - I_color.getWidth(), I_color.getHeight(), - false); + vpImageConvert::RGBToGrey(rgb_array, rgb2gray_array_sse, I_color.getWidth(), I_color.getHeight(), false); value_sse += rgb2gray_array_sse[0]; } t_sse = vpTime::measureTimeMs() - t_sse; - unsigned char *rgb2gray_array_regular = - new unsigned char[I_color.getSize()]; + unsigned char *rgb2gray_array_regular = new unsigned char[I_color.getSize()]; t_regular = vpTime::measureTimeMs(); for (int iteration = 0; iteration < nbIterations; iteration++) { - computeRegularRGBToGrayscale(rgb_array, rgb2gray_array_regular, - I_color.getSize()); + computeRegularRGBToGrayscale(rgb_array, rgb2gray_array_regular, I_color.getSize()); value_regular += rgb2gray_array_regular[0]; } t_regular = vpTime::measureTimeMs() - t_regular; - vpImage I_gray2rgba_sse( - rgb2gray_array_sse, I_color.getHeight(), I_color.getWidth(), false); - vpImage I_gray2rgba_regular(rgb2gray_array_regular, - I_color.getHeight(), - I_color.getWidth(), false); + vpImage I_gray2rgba_sse(rgb2gray_array_sse, I_color.getHeight(), I_color.getWidth(), false); + vpImage I_gray2rgba_regular(rgb2gray_array_regular, I_color.getHeight(), I_color.getWidth(), + false); // Compute the error between the SSE and regular version rmse_error = 0.0; for (unsigned int i = 0; i < I_color.getHeight(); i++) { for (unsigned int j = 0; j < I_color.getWidth(); j++) { - rmse_error += (I_gray2rgba_sse[i][j] - I_gray2rgba_regular[i][j]) * - (I_gray2rgba_sse[i][j] - I_gray2rgba_regular[i][j]); + rmse_error += + (I_gray2rgba_sse[i][j] - I_gray2rgba_regular[i][j]) * (I_gray2rgba_sse[i][j] - I_gray2rgba_regular[i][j]); } } std::cout << "\n RGB to Grayscale" << std::endl; - std::cout << " t_regular (" << nbIterations - << " iterations)=" << t_regular << " ms" - << " ; t_sse (" << nbIterations << " iterations)=" << t_sse - << " ms" << std::endl; + std::cout << " t_regular (" << nbIterations << " iterations)=" << t_regular << " ms" + << " ; t_sse (" << nbIterations << " iterations)=" << t_sse << " ms" << std::endl; std::cout << " Speed-up=" << (t_regular / t_sse) << "X" << std::endl; - std::cout << " RMSE error between SSE and regular version: " - << (std::sqrt(rmse_error / I_color.getSize())) << std::endl; + std::cout << " RMSE error between SSE and regular version: " << (std::sqrt(rmse_error / I_color.getSize())) + << std::endl; // To prevent the iteration loop to not be optimized? std::cout << " value_sse=" << static_cast(value_sse) - << " ; value_regular=" << static_cast(value_regular) - << std::endl; + << " ; value_regular=" << static_cast(value_regular) << std::endl; filename = vpIoTools::createFilePath(opath, "I_rgb2gray_sse.pgm"); std::cout << " Resulting image saved in: " << filename << std::endl; @@ -927,8 +852,7 @@ int main(int argc, const char **argv) std::cout << "\n BGR cv::Mat to Grayscale" << std::endl; filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm"); cv::Mat colorMat = cv::imread(filename); - std::cout << " colorMat=" << colorMat.cols << "x" << colorMat.rows - << std::endl; + std::cout << " colorMat=" << colorMat.cols << "x" << colorMat.rows << std::endl; vpImage I_mat2gray_sse, I_mat2gray_regular; value_sse = 0; @@ -952,23 +876,20 @@ int main(int argc, const char **argv) rmse_error = 0.0; for (unsigned int i = 0; i < I_color.getHeight(); i++) { for (unsigned int j = 0; j < I_color.getWidth(); j++) { - rmse_error += (I_mat2gray_sse[i][j] - I_mat2gray_regular[i][j]) * - (I_mat2gray_sse[i][j] - I_mat2gray_regular[i][j]); + rmse_error += + (I_mat2gray_sse[i][j] - I_mat2gray_regular[i][j]) * (I_mat2gray_sse[i][j] - I_mat2gray_regular[i][j]); } } - std::cout << " t_regular (" << nbIterations - << " iterations)=" << t_regular << " ms" - << " ; t_sse (" << nbIterations << " iterations)=" << t_sse - << " ms" << std::endl; + std::cout << " t_regular (" << nbIterations << " iterations)=" << t_regular << " ms" + << " ; t_sse (" << nbIterations << " iterations)=" << t_sse << " ms" << std::endl; std::cout << " Speed-up=" << (t_regular / t_sse) << "X" << std::endl; - std::cout << " RMSE error between SSE and regular version: " - << (std::sqrt(rmse_error / I_color.getSize())) << std::endl; + std::cout << " RMSE error between SSE and regular version: " << (std::sqrt(rmse_error / I_color.getSize())) + << std::endl; // To prevent the iteration loop to not be optimized? std::cout << " value_sse=" << static_cast(value_sse) - << " ; value_regular=" << static_cast(value_regular) - << std::endl; + << " ; value_regular=" << static_cast(value_regular) << std::endl; filename = vpIoTools::createFilePath(opath, "I_mat2gray_sse.pgm"); std::cout << " Resulting image saved in: " << filename << std::endl; @@ -990,13 +911,10 @@ int main(int argc, const char **argv) } t_opencv = vpTime::measureTimeMs() - t_opencv; - std::cout << " t_opencv (" << nbIterations - << " iterations)=" << t_opencv << " ms" - << " ; t_sse (" << nbIterations << " iterations)=" << t_sse - << " ms" << std::endl; + std::cout << " t_opencv (" << nbIterations << " iterations)=" << t_opencv << " ms" + << " ; t_sse (" << nbIterations << " iterations)=" << t_sse << " ms" << std::endl; std::cout << " Speed-up=" << (t_opencv / t_sse) << "X" << std::endl; - std::cout << " value_mat=" << static_cast(value_mat) - << std::endl; + std::cout << " value_mat=" << static_cast(value_mat) << std::endl; vpImage I_grayscale_mat; vpImageConvert::convert(grayscaleMat, I_grayscale_mat); @@ -1006,13 +924,9 @@ int main(int argc, const char **argv) // Test RGB to Grayscale + Flip std::cout << "\n RGB to Grayscale + Flip" << std::endl; - unsigned char *rgb2gray_flip_array_sse = - new unsigned char[I_color.getSize()]; - vpImageConvert::RGBToGrey(rgb_array, rgb2gray_flip_array_sse, - I_color.getWidth(), I_color.getHeight(), - true); - vpImage I_rgb2gray_flip_sse( - rgb2gray_flip_array_sse, I_color.getHeight(), I_color.getWidth()); + unsigned char *rgb2gray_flip_array_sse = new unsigned char[I_color.getSize()]; + vpImageConvert::RGBToGrey(rgb_array, rgb2gray_flip_array_sse, I_color.getWidth(), I_color.getHeight(), true); + vpImage I_rgb2gray_flip_sse(rgb2gray_flip_array_sse, I_color.getHeight(), I_color.getWidth()); filename = vpIoTools::createFilePath(opath, "I_rgb2gray_flip_sse.pgm"); std::cout << " Resulting image saved in: " << filename << std::endl; @@ -1020,10 +934,8 @@ int main(int argc, const char **argv) // Test BGR to Grayscale + Flip std::cout << "\n Conversion BGR to Grayscale + Flip" << std::endl; - unsigned char *bgr2gray_flip_array_sse = - new unsigned char[I_color.getSize()]; - vpImage I_bgr2gray_flip_sse( - bgr2gray_flip_array_sse, I_color.getHeight(), I_color.getWidth()); + unsigned char *bgr2gray_flip_array_sse = new unsigned char[I_color.getSize()]; + vpImage I_bgr2gray_flip_sse(bgr2gray_flip_array_sse, I_color.getHeight(), I_color.getWidth()); vpImageConvert::convert(colorMat, I_bgr2gray_flip_sse, true); filename = vpIoTools::createFilePath(opath, "I_bgr2gray_flip_sse.pgm"); @@ -1035,70 +947,50 @@ int main(int argc, const char **argv) cv::Rect rect_roi(11, 17, 347, 449); cv::Mat colorMat_crop = colorMat(rect_roi); cv::Mat colorMat_crop_continous = colorMat(rect_roi).clone(); - std::cout << " colorMat_crop: " << colorMat_crop.cols << "x" - << colorMat_crop.rows << " is continuous? " + std::cout << " colorMat_crop: " << colorMat_crop.cols << "x" << colorMat_crop.rows << " is continuous? " << colorMat_crop.isContinuous() << std::endl; - std::cout << " colorMat_crop_continous: " - << colorMat_crop_continous.cols << "x" - << colorMat_crop_continous.rows << " is continuous? " - << colorMat_crop_continous.isContinuous() << std::endl; - - vpImage I_color_crop( - (unsigned int)(rect_roi.height - rect_roi.y), - (unsigned int)(rect_roi.width - rect_roi.x)); - for (unsigned int i = (unsigned int)rect_roi.y; - i < (unsigned int)rect_roi.height; i++) { - for (unsigned int j = (unsigned int)rect_roi.x; - j < (unsigned int)rect_roi.width; j++) { - I_color_crop[(unsigned int)((int)i - rect_roi.y)] - [(unsigned int)((int)j - rect_roi.x)] = I_color[i][j]; + std::cout << " colorMat_crop_continous: " << colorMat_crop_continous.cols << "x" << colorMat_crop_continous.rows + << " is continuous? " << colorMat_crop_continous.isContinuous() << std::endl; + + vpImage I_color_crop((unsigned int)(rect_roi.height - rect_roi.y), + (unsigned int)(rect_roi.width - rect_roi.x)); + for (unsigned int i = (unsigned int)rect_roi.y; i < (unsigned int)rect_roi.height; i++) { + for (unsigned int j = (unsigned int)rect_roi.x; j < (unsigned int)rect_roi.width; j++) { + I_color_crop[(unsigned int)((int)i - rect_roi.y)][(unsigned int)((int)j - rect_roi.x)] = I_color[i][j]; } } filename = vpIoTools::createFilePath(opath, "I_color_crop.ppm"); std::cout << " Resulting image saved in: " << filename << std::endl; vpImageIo::write(I_color_crop, filename); - unsigned char *rgb_array_crop = - new unsigned char[I_color_crop.getSize() * 3]; - vpImageConvert::RGBaToRGB((unsigned char *)I_color_crop.bitmap, - rgb_array_crop, I_color_crop.getSize()); + unsigned char *rgb_array_crop = new unsigned char[I_color_crop.getSize() * 3]; + vpImageConvert::RGBaToRGB((unsigned char *)I_color_crop.bitmap, rgb_array_crop, I_color_crop.getSize()); - unsigned char *rgb2gray_flip_crop_array_sse = - new unsigned char[I_color_crop.getSize()]; - vpImageConvert::RGBToGrey(rgb_array_crop, rgb2gray_flip_crop_array_sse, - I_color_crop.getWidth(), + unsigned char *rgb2gray_flip_crop_array_sse = new unsigned char[I_color_crop.getSize()]; + vpImageConvert::RGBToGrey(rgb_array_crop, rgb2gray_flip_crop_array_sse, I_color_crop.getWidth(), I_color_crop.getHeight(), true); - vpImage I_rgb2gray_flip_crop_sse( - rgb2gray_flip_crop_array_sse, I_color_crop.getHeight(), - I_color_crop.getWidth()); + vpImage I_rgb2gray_flip_crop_sse(rgb2gray_flip_crop_array_sse, I_color_crop.getHeight(), + I_color_crop.getWidth()); - filename = - vpIoTools::createFilePath(opath, "I_rgb2gray_flip_crop_sse.pgm"); + filename = vpIoTools::createFilePath(opath, "I_rgb2gray_flip_crop_sse.pgm"); std::cout << " Resulting image saved in: " << filename << std::endl; vpImageIo::write(I_rgb2gray_flip_crop_sse, filename); // Test BGR to Grayscale + Flip + Crop std::cout << "\n BGR to Grayscale + Flip + Crop" << std::endl; - vpImage I_bgr2gray_flip_crop_sse( - I_color_crop.getHeight(), I_color_crop.getWidth()); - vpImageConvert::convert(colorMat_crop_continous, - I_bgr2gray_flip_crop_sse, true); + vpImage I_bgr2gray_flip_crop_sse(I_color_crop.getHeight(), I_color_crop.getWidth()); + vpImageConvert::convert(colorMat_crop_continous, I_bgr2gray_flip_crop_sse, true); - filename = - vpIoTools::createFilePath(opath, "I_bgr2gray_flip_crop_sse.pgm"); + filename = vpIoTools::createFilePath(opath, "I_bgr2gray_flip_crop_sse.pgm"); std::cout << " Resulting image saved in: " << filename << std::endl; vpImageIo::write(I_bgr2gray_flip_crop_sse, filename); // Test BGR to Grayscale + Flip + Crop + No continuous Mat - std::cout << "\n BGR to Grayscale + Flip + Crop + No continuous Mat" - << std::endl; - vpImage I_bgr2gray_flip_crop_no_continuous_sse( - I_color_crop.getHeight(), I_color_crop.getWidth()); - vpImageConvert::convert(colorMat_crop, - I_bgr2gray_flip_crop_no_continuous_sse, true); + std::cout << "\n BGR to Grayscale + Flip + Crop + No continuous Mat" << std::endl; + vpImage I_bgr2gray_flip_crop_no_continuous_sse(I_color_crop.getHeight(), I_color_crop.getWidth()); + vpImageConvert::convert(colorMat_crop, I_bgr2gray_flip_crop_no_continuous_sse, true); - filename = vpIoTools::createFilePath( - opath, "I_bgr2gray_flip_crop_no_continuous_sse.pgm"); + filename = vpIoTools::createFilePath(opath, "I_bgr2gray_flip_crop_no_continuous_sse.pgm"); std::cout << " Resulting image saved in: " << filename << std::endl; vpImageIo::write(I_bgr2gray_flip_crop_no_continuous_sse, filename); diff --git a/modules/core/test/image/testCrop.cpp b/modules/core/test/image/testCrop.cpp index 510b8e503d..dfa8f257e9 100644 --- a/modules/core/test/image/testCrop.cpp +++ b/modules/core/test/image/testCrop.cpp @@ -70,8 +70,8 @@ \param user : Username. */ -void usage(const char *name, const char *badparam, const std::string &ipath, - const std::string &opath, const std::string &user) +void usage(const char *name, const char *badparam, const std::string &ipath, const std::string &opath, + const std::string &user) { fprintf(stdout, "\n\ Read an image from the disk (Klimt.pgm), crop a rectangular area\n\ @@ -117,8 +117,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, const std::string &user) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, const std::string &user) { const char *optarg_; int c; @@ -210,8 +209,7 @@ int main(int argc, const char **argv) 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; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; exit(-1); } } @@ -222,8 +220,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -232,13 +229,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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); } diff --git a/modules/core/test/image/testCropAdvanced.cpp b/modules/core/test/image/testCropAdvanced.cpp index c0c30a2922..cb99bc38fe 100644 --- a/modules/core/test/image/testCropAdvanced.cpp +++ b/modules/core/test/image/testCropAdvanced.cpp @@ -64,8 +64,8 @@ \param user : Username. */ -void usage(const char *name, const char *badparam, const std::string &ipath, - const std::string &opath, const std::string &user) +void usage(const char *name, const char *badparam, const std::string &ipath, const std::string &opath, + const std::string &user) { fprintf(stdout, "\n\ Read an image from the disk (Klimt.pgm and klimp.ppm), crop a rectangular area\n\ @@ -109,8 +109,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, const std::string &user) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, const std::string &user) { const char *optarg_; int c; @@ -201,8 +200,7 @@ int main(int argc, const char **argv) 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; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; exit(-1); } } @@ -213,8 +211,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -223,13 +220,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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); } @@ -237,44 +231,35 @@ int main(int argc, const char **argv) vpImage I; // Read the input grey image from the disk - std::string filename = - vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm"); + std::string filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm"); std::cout << "Read image: " << filename << std::endl; vpImageIo::read(I, filename); - std::cout << "Image size: " << I.getWidth() << " " << I.getHeight() - << std::endl; + std::cout << "Image size: " << I.getWidth() << " " << I.getHeight() << std::endl; std::vector > reference; vpImage crop; vpImageTools::crop(I, 100, 100, 640, 640, crop); reference.push_back(crop); - filename = - vpIoTools::createFilePath(opath, "Klimt_uchar_ref_crop-1x1.pgm"); - std::cout << " Cropped image reference saved in: " << filename - << std::endl; + filename = vpIoTools::createFilePath(opath, "Klimt_uchar_ref_crop-1x1.pgm"); + std::cout << " Cropped image reference saved in: " << filename << std::endl; vpImageIo::write(crop, filename); vpImageTools::crop(I, 100, 100, 640, 640, crop, 2, 1); reference.push_back(crop); - filename = - vpIoTools::createFilePath(opath, "Klimt_uchar_ref_crop-2x1.pgm"); - std::cout << " Cropped image reference saved in: " << filename - << std::endl; + filename = vpIoTools::createFilePath(opath, "Klimt_uchar_ref_crop-2x1.pgm"); + std::cout << " Cropped image reference saved in: " << filename << std::endl; vpImageIo::write(crop, filename); vpImageTools::crop(I, 100, 100, 640, 640, crop, 2, 2); reference.push_back(crop); - filename = - vpIoTools::createFilePath(opath, "Klimt_uchar_ref_crop-2x2.pgm"); - std::cout << " Cropped image reference saved in: " << filename - << std::endl; + filename = vpIoTools::createFilePath(opath, "Klimt_uchar_ref_crop-2x2.pgm"); + std::cout << " Cropped image reference saved in: " << filename << std::endl; vpImageIo::write(crop, filename); vpRect roi(100, 100, 640, 640); vpImageTools::crop(I, roi, crop); - filename = - vpIoTools::createFilePath(opath, "Klimt_uchar_roi_crop-1x1.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_uchar_roi_crop-1x1.pgm"); std::cout << " Cropped image saved in: " << filename << std::endl; vpImageIo::write(crop, filename); if (crop != reference[0]) { @@ -283,8 +268,7 @@ int main(int argc, const char **argv) } vpImageTools::crop(I, roi, crop, 2, 1); - filename = - vpIoTools::createFilePath(opath, "Klimt_uchar_roi_crop-2x1.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_uchar_roi_crop-2x1.pgm"); std::cout << " Cropped image saved in: " << filename << std::endl; vpImageIo::write(crop, filename); if (crop != reference[1]) { @@ -293,8 +277,7 @@ int main(int argc, const char **argv) } vpImageTools::crop(I, roi, crop, 2, 2); - filename = - vpIoTools::createFilePath(opath, "Klimt_uchar_roi_crop-2x2.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_uchar_roi_crop-2x2.pgm"); std::cout << " Cropped image saved in: " << filename << std::endl; vpImageIo::write(crop, filename); if (crop != reference[2]) { @@ -303,8 +286,7 @@ int main(int argc, const char **argv) } vpImageTools::crop(I.bitmap, I.getWidth(), I.getHeight(), roi, crop); - filename = - vpIoTools::createFilePath(opath, "Klimt_uchar_bitmap_crop-1x1.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_uchar_bitmap_crop-1x1.pgm"); std::cout << " Cropped image saved in: " << filename << std::endl; vpImageIo::write(crop, filename); if (crop != reference[0]) { @@ -312,10 +294,8 @@ int main(int argc, const char **argv) return -1; } - vpImageTools::crop(I.bitmap, I.getWidth(), I.getHeight(), roi, crop, 2, - 1); - filename = - vpIoTools::createFilePath(opath, "Klimt_uchar_bitmap_crop-2x1.pgm"); + vpImageTools::crop(I.bitmap, I.getWidth(), I.getHeight(), roi, crop, 2, 1); + filename = vpIoTools::createFilePath(opath, "Klimt_uchar_bitmap_crop-2x1.pgm"); std::cout << " Cropped image saved in: " << filename << std::endl; vpImageIo::write(crop, filename); if (crop != reference[1]) { @@ -323,10 +303,8 @@ int main(int argc, const char **argv) return -1; } - vpImageTools::crop(I.bitmap, I.getWidth(), I.getHeight(), roi, crop, 2, - 2); - filename = - vpIoTools::createFilePath(opath, "Klimt_uchar_bitmap_crop-2x2.pgm"); + vpImageTools::crop(I.bitmap, I.getWidth(), I.getHeight(), roi, crop, 2, 2); + filename = vpIoTools::createFilePath(opath, "Klimt_uchar_bitmap_crop-2x2.pgm"); std::cout << " Cropped image saved in: " << filename << std::endl; vpImageIo::write(crop, filename); if (crop != reference[2]) { @@ -339,12 +317,10 @@ int main(int argc, const char **argv) vpImage I; // Read the input color image from the disk - std::string filename = - vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm"); + std::string filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm"); std::cout << "Read image: " << filename << std::endl; vpImageIo::read(I, filename); - std::cout << "Image size: " << I.getWidth() << " " << I.getHeight() - << std::endl; + std::cout << "Image size: " << I.getWidth() << " " << I.getHeight() << std::endl; vpImage crop; @@ -352,32 +328,25 @@ int main(int argc, const char **argv) vpImageTools::crop(I, 100, 100, 640, 640, crop); reference.push_back(crop); - filename = - vpIoTools::createFilePath(opath, "Klimt_rgba_ref_crop-1x1.pgm"); - std::cout << " Cropped image reference saved in: " << filename - << std::endl; + filename = vpIoTools::createFilePath(opath, "Klimt_rgba_ref_crop-1x1.pgm"); + std::cout << " Cropped image reference saved in: " << filename << std::endl; vpImageIo::write(crop, filename); vpImageTools::crop(I, 100, 100, 640, 640, crop, 2, 1); reference.push_back(crop); - filename = - vpIoTools::createFilePath(opath, "Klimt_rgba_ref_crop-2x1.pgm"); - std::cout << " Cropped image reference saved in: " << filename - << std::endl; + filename = vpIoTools::createFilePath(opath, "Klimt_rgba_ref_crop-2x1.pgm"); + std::cout << " Cropped image reference saved in: " << filename << std::endl; vpImageIo::write(crop, filename); vpImageTools::crop(I, 100, 100, 640, 640, crop, 2, 2); reference.push_back(crop); - filename = - vpIoTools::createFilePath(opath, "Klimt_rgba_ref_crop-2x2.pgm"); - std::cout << " Cropped image reference saved in: " << filename - << std::endl; + filename = vpIoTools::createFilePath(opath, "Klimt_rgba_ref_crop-2x2.pgm"); + std::cout << " Cropped image reference saved in: " << filename << std::endl; vpImageIo::write(crop, filename); vpRect roi(100, 100, 640, 640); vpImageTools::crop(I, roi, crop); - filename = - vpIoTools::createFilePath(opath, "Klimt_rgba_roi_crop-1x1.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_rgba_roi_crop-1x1.pgm"); std::cout << " Cropped image saved in: " << filename << std::endl; vpImageIo::write(crop, filename); if (crop != reference[0]) { @@ -386,8 +355,7 @@ int main(int argc, const char **argv) } vpImageTools::crop(I, roi, crop, 2, 1); - filename = - vpIoTools::createFilePath(opath, "Klimt_rgba_roi_crop-2x1.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_rgba_roi_crop-2x1.pgm"); std::cout << " Cropped image saved in: " << filename << std::endl; vpImageIo::write(crop, filename); if (crop != reference[1]) { @@ -396,8 +364,7 @@ int main(int argc, const char **argv) } vpImageTools::crop(I, roi, crop, 2, 2); - filename = - vpIoTools::createFilePath(opath, "Klimt_rgba_roi_crop-2x2.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_rgba_roi_crop-2x2.pgm"); std::cout << " Cropped image saved in: " << filename << std::endl; vpImageIo::write(crop, filename); if (crop != reference[2]) { @@ -405,10 +372,8 @@ int main(int argc, const char **argv) return -1; } - vpImageTools::crop((unsigned char *)I.bitmap, I.getWidth(), - I.getHeight(), roi, crop); - filename = - vpIoTools::createFilePath(opath, "Klimt_rgba_bitmap_crop-1x1.pgm"); + vpImageTools::crop((unsigned char *)I.bitmap, I.getWidth(), I.getHeight(), roi, crop); + filename = vpIoTools::createFilePath(opath, "Klimt_rgba_bitmap_crop-1x1.pgm"); std::cout << " Cropped image saved in: " << filename << std::endl; vpImageIo::write(crop, filename); if (crop != reference[0]) { @@ -416,10 +381,8 @@ int main(int argc, const char **argv) return -1; } - vpImageTools::crop((unsigned char *)I.bitmap, I.getWidth(), - I.getHeight(), roi, crop, 2, 1); - filename = - vpIoTools::createFilePath(opath, "Klimt_rgba_bitmap_crop-2x1.pgm"); + vpImageTools::crop((unsigned char *)I.bitmap, I.getWidth(), I.getHeight(), roi, crop, 2, 1); + filename = vpIoTools::createFilePath(opath, "Klimt_rgba_bitmap_crop-2x1.pgm"); std::cout << " Cropped image saved in: " << filename << std::endl; vpImageIo::write(crop, filename); if (crop != reference[1]) { @@ -427,10 +390,8 @@ int main(int argc, const char **argv) return -1; } - vpImageTools::crop((unsigned char *)I.bitmap, I.getWidth(), - I.getHeight(), roi, crop, 2, 2); - filename = - vpIoTools::createFilePath(opath, "Klimt_rgba_bitmap_crop-2x2.pgm"); + vpImageTools::crop((unsigned char *)I.bitmap, I.getWidth(), I.getHeight(), roi, crop, 2, 2); + filename = vpIoTools::createFilePath(opath, "Klimt_rgba_bitmap_crop-2x2.pgm"); std::cout << " Cropped image saved in: " << filename << std::endl; vpImageIo::write(crop, filename); if (crop != reference[2]) { diff --git a/modules/core/test/image/testImageAddSub.cpp b/modules/core/test/image/testImageAddSub.cpp index 3d67f83271..be5c93486a 100644 --- a/modules/core/test/image/testImageAddSub.cpp +++ b/modules/core/test/image/testImageAddSub.cpp @@ -62,8 +62,8 @@ \param user : Username. \param nbiter : Number of benchmark iterations. */ -void usage(const char *name, const char *badparam, const std::string &ipath, - const std::string &opath, const std::string &user, int nbiter) +void usage(const char *name, const char *badparam, const std::string &ipath, const std::string &opath, + const std::string &user, int nbiter) { fprintf(stdout, "\n\ Test images addition / substraction.\n\ @@ -110,8 +110,8 @@ OPTIONS: Default\n\ \param nbiter : Number of benchmark iterations. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, const std::string &user, int &nbiter) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, const std::string &user, + int &nbiter) { const char *optarg_; int c; @@ -154,18 +154,14 @@ bool getOptions(int argc, const char **argv, std::string &ipath, return true; } -void regularImageAdd(const vpImage &I1, - const vpImage &I2, - vpImage &Ires, const bool saturate) +void regularImageAdd(const vpImage &I1, const vpImage &I2, vpImage &Ires, + const bool saturate) { - 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() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) { + throw(vpException(vpException::dimensionError, "The two images do not have the same size")); } - if ((I1.getHeight() != Ires.getHeight()) || - (I1.getWidth() != Ires.getWidth())) { + if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) { Ires.resize(I1.getHeight(), I1.getWidth()); } @@ -173,26 +169,19 @@ void regularImageAdd(const vpImage &I1, unsigned char *ptr_I2 = I2.bitmap; unsigned char *ptr_Ires = Ires.bitmap; - for (unsigned int cpt = 0; cpt < Ires.getSize(); - cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) { - *ptr_Ires = saturate ? vpMath::saturate( - (short int)*ptr_I1 + (short int)*ptr_I2) - : *ptr_I1 + *ptr_I2; + for (unsigned int cpt = 0; cpt < Ires.getSize(); cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) { + *ptr_Ires = saturate ? vpMath::saturate((short int)*ptr_I1 + (short int)*ptr_I2) : *ptr_I1 + *ptr_I2; } } -void regularImageSubtract(const vpImage &I1, - const vpImage &I2, +void regularImageSubtract(const vpImage &I1, const vpImage &I2, vpImage &Ires, const bool saturate) { - 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() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) { + throw(vpException(vpException::dimensionError, "The two images do not have the same size")); } - if ((I1.getHeight() != Ires.getHeight()) || - (I1.getWidth() != Ires.getWidth())) { + if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) { Ires.resize(I1.getHeight(), I1.getWidth()); } @@ -200,11 +189,8 @@ void regularImageSubtract(const vpImage &I1, unsigned char *ptr_I2 = I2.bitmap; unsigned char *ptr_Ires = Ires.bitmap; - for (unsigned int cpt = 0; cpt < Ires.getSize(); - cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) { - *ptr_Ires = saturate ? vpMath::saturate( - (short int)*ptr_I1 - (short int)*ptr_I2) - : *ptr_I1 - *ptr_I2; + for (unsigned int cpt = 0; cpt < Ires.getSize(); cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) { + *ptr_Ires = saturate ? vpMath::saturate((short int)*ptr_I1 - (short int)*ptr_I2) : *ptr_I1 - *ptr_I2; } } @@ -239,8 +225,7 @@ int main(int argc, const char **argv) vpIoTools::getUserName(username); // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_opath, username, - nbIterations) == false) { + if (getOptions(argc, argv, opt_ipath, opt_opath, username, nbIterations) == false) { exit(EXIT_FAILURE); } @@ -262,8 +247,7 @@ int main(int argc, const char **argv) usage(argv[0], NULL, ipath, opt_opath, username, nbIterations); std::cerr << std::endl << "ERROR:" << std::endl; std::cerr << " Cannot create " << opath << std::endl; - std::cerr << " Check your -o " << opt_opath << " option " - << std::endl; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; exit(EXIT_FAILURE); } } @@ -274,8 +258,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -284,13 +267,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, opt_opath, username, nbIterations); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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(EXIT_FAILURE); } @@ -309,8 +289,7 @@ int main(int argc, const char **argv) vpImage Iadd; vpImageTools::imageAdd(I, Inull, Iadd); if (Iadd != I) { - throw vpException(vpException::fatalError, - "Problem with vpImageTools::imageAdd (Iadd != I)!"); + throw vpException(vpException::fatalError, "Problem with vpImageTools::imageAdd (Iadd != I)!"); } else { std::cout << "(Iadd == I)? " << (Iadd == I) << std::endl; } @@ -319,9 +298,7 @@ int main(int argc, const char **argv) Iadd = 0; vpImageTools::imageAdd(I, Iadd, Iadd); if (Iadd != I) { - throw vpException( - vpException::fatalError, - "Problem with in-place vpImageTools::imageAdd (Iadd != I)!"); + throw vpException(vpException::fatalError, "Problem with in-place vpImageTools::imageAdd (Iadd != I)!"); } else { std::cout << "In-place (Iadd == I)? " << (Iadd == I) << std::endl; } @@ -331,9 +308,7 @@ int main(int argc, const char **argv) vpImage Isub; vpImageTools::imageSubtract(I, Inull, Isub); if (Isub != I) { - throw vpException( - vpException::fatalError, - "Problem with vpImageTools::imageSubtract (Iadd != I)!"); + throw vpException(vpException::fatalError, "Problem with vpImageTools::imageSubtract (Iadd != I)!"); } else { std::cout << "(Isub == I)? " << (Isub == I) << std::endl; } @@ -342,9 +317,7 @@ int main(int argc, const char **argv) Isub = 0; vpImageTools::imageSubtract(I, Isub, Isub); if (Isub != I) { - throw vpException( - vpException::fatalError, - "Problem with in-place vpImageTools::imageSubtract (Isub != I)!"); + throw vpException(vpException::fatalError, "Problem with in-place vpImageTools::imageSubtract (Isub != I)!"); } else { std::cout << "In-place (Isub == I)? " << (Isub == I) << std::endl; } @@ -360,24 +333,20 @@ int main(int argc, const char **argv) vpImage Iadd_regular; regularImageAdd(I, I2, Iadd_regular, false); if (Iadd != Iadd_regular) { - throw vpException(vpException::fatalError, - "Problem with vpImageTools::imageAdd(I, I2, Iadd, " - "false) (Iadd != Iadd_regular)!"); + throw vpException(vpException::fatalError, "Problem with vpImageTools::imageAdd(I, I2, Iadd, " + "false) (Iadd != Iadd_regular)!"); } else { - std::cout << "\nNo saturation (Iadd == Iadd_regular)? " - << (Iadd == Iadd_regular) << std::endl; + std::cout << "\nNo saturation (Iadd == Iadd_regular)? " << (Iadd == Iadd_regular) << std::endl; } // Saturation vpImageTools::imageAdd(I, I2, Iadd, true); regularImageAdd(I, I2, Iadd_regular, true); if (Iadd != Iadd_regular) { - throw vpException(vpException::fatalError, - "Problem with vpImageTools::imageAdd(I, I2, Iadd, " - "true) (Iadd != Iadd_regular)!"); + throw vpException(vpException::fatalError, "Problem with vpImageTools::imageAdd(I, I2, Iadd, " + "true) (Iadd != Iadd_regular)!"); } else { - std::cout << "Saturation (Iadd == Iadd_regular)? " - << (Iadd == Iadd_regular) << std::endl; + std::cout << "Saturation (Iadd == Iadd_regular)? " << (Iadd == Iadd_regular) << std::endl; } // Test subtract image saturation @@ -386,24 +355,20 @@ int main(int argc, const char **argv) vpImage Isub_regular; regularImageSubtract(I, I2, Isub_regular, false); if (Isub != Isub_regular) { - throw vpException(vpException::fatalError, - "Problem with vpImageTools::imageSubtract(I, I2, " - "Isub, false) (Isub != Isub_regular)!"); + throw vpException(vpException::fatalError, "Problem with vpImageTools::imageSubtract(I, I2, " + "Isub, false) (Isub != Isub_regular)!"); } else { - std::cout << "\nNo saturation (Isub == Isub_regular)? " - << (Isub == Isub_regular) << std::endl; + std::cout << "\nNo saturation (Isub == Isub_regular)? " << (Isub == Isub_regular) << std::endl; } // Saturation vpImageTools::imageSubtract(I, I2, Isub, true); regularImageSubtract(I, I2, Isub_regular, true); if (Isub != Isub_regular) { - throw vpException(vpException::fatalError, - "Problem with vpImageTools::imageSubtract(I, I2, " - "Isub, true) (Isub != Isub_regular)!"); + throw vpException(vpException::fatalError, "Problem with vpImageTools::imageSubtract(I, I2, " + "Isub, true) (Isub != Isub_regular)!"); } else { - std::cout << "Saturation (Isub == Isub_regular)? " - << (Isub == Isub_regular) << std::endl; + std::cout << "Saturation (Isub == Isub_regular)? " << (Isub == Isub_regular) << std::endl; } // Benchmark @@ -414,8 +379,7 @@ int main(int argc, const char **argv) vpImageTools::imageAdd(I, Iadd, Iadd, false); } t_sse = vpTime::measureTimeMs() - t_sse; - std::cout << "\nAdd no saturation ; t_sse (" << nbIterations - << " iterations)=" << t_sse << " ms" << std::endl; + std::cout << "\nAdd no saturation ; t_sse (" << nbIterations << " iterations)=" << t_sse << " ms" << std::endl; Iadd_regular = 0; double t = vpTime::measureTimeMs(); @@ -423,11 +387,9 @@ int main(int argc, const char **argv) regularImageAdd(I, Iadd_regular, Iadd_regular, false); } t = vpTime::measureTimeMs() - t; - std::cout << "Add regular no saturation ; t (" << nbIterations - << " iterations)=" << t << " ms" + std::cout << "Add regular no saturation ; t (" << nbIterations << " iterations)=" << t << " ms" << " ; Speed-up: " << (t / t_sse) << "X" << std::endl; - std::cout << "(Iadd == Iadd_regular)? " << (Iadd == Iadd_regular) - << std::endl; + std::cout << "(Iadd == Iadd_regular)? " << (Iadd == Iadd_regular) << std::endl; // Write add no saturation filename = vpIoTools::createFilePath(opath, "Klimt_add_no_sat_sse.pgm"); @@ -445,8 +407,7 @@ int main(int argc, const char **argv) vpImageTools::imageAdd(I, Iadd, Iadd, true); } t_sse = vpTime::measureTimeMs() - t_sse; - std::cout << "\nAdd saturation ; t_sse (" << nbIterations - << " iterations)=" << t_sse << " ms" << std::endl; + std::cout << "\nAdd saturation ; t_sse (" << nbIterations << " iterations)=" << t_sse << " ms" << std::endl; Iadd_regular = 0; t = vpTime::measureTimeMs(); @@ -454,11 +415,9 @@ int main(int argc, const char **argv) regularImageAdd(I, Iadd_regular, Iadd_regular, true); } t = vpTime::measureTimeMs() - t; - std::cout << "Add saturation ; t (" << nbIterations - << " iterations)=" << t << " ms" + std::cout << "Add saturation ; t (" << nbIterations << " iterations)=" << t << " ms" << " ; Speed-up: " << (t / t_sse) << "X" << std::endl; - std::cout << "(Iadd == Iadd_regular)? " << (Iadd == Iadd_regular) - << std::endl; + std::cout << "(Iadd == Iadd_regular)? " << (Iadd == Iadd_regular) << std::endl; // Write add no saturation filename = vpIoTools::createFilePath(opath, "Klimt_add_sat_sse.pgm"); @@ -476,8 +435,7 @@ int main(int argc, const char **argv) vpImageTools::imageSubtract(I, Isub, Isub, false); } t_sse = vpTime::measureTimeMs() - t_sse; - std::cout << "\nSubtract no saturation ; t_sse (" << nbIterations - << " iterations)=" << t_sse << " ms" << std::endl; + std::cout << "\nSubtract no saturation ; t_sse (" << nbIterations << " iterations)=" << t_sse << " ms" << std::endl; Isub_regular = I2; t = vpTime::measureTimeMs(); @@ -485,15 +443,12 @@ int main(int argc, const char **argv) regularImageSubtract(I, Isub_regular, Isub_regular, false); } t = vpTime::measureTimeMs() - t; - std::cout << "Subtract regular no saturation ; t (" << nbIterations - << " iterations)=" << t << " ms" + std::cout << "Subtract regular no saturation ; t (" << nbIterations << " iterations)=" << t << " ms" << " ; Speed-up: " << (t / t_sse) << "X" << std::endl; - std::cout << "(Isub == Isub_regular)? " << (Isub == Isub_regular) - << std::endl; + std::cout << "(Isub == Isub_regular)? " << (Isub == Isub_regular) << std::endl; // Write subtract no saturation - filename = - vpIoTools::createFilePath(opath, "Klimt_subtract_no_sat_sse.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_subtract_no_sat_sse.pgm"); std::cout << "\nWrite: " << filename << std::endl; vpImageIo::write(Isub, filename); @@ -508,8 +463,7 @@ int main(int argc, const char **argv) vpImageTools::imageSubtract(I, Isub, Isub, true); } t_sse = vpTime::measureTimeMs() - t_sse; - std::cout << "\nSubtract saturation ; t_sse (" << nbIterations - << " iterations)=" << t_sse << " ms" << std::endl; + std::cout << "\nSubtract saturation ; t_sse (" << nbIterations << " iterations)=" << t_sse << " ms" << std::endl; Isub_regular = I2; t = vpTime::measureTimeMs(); @@ -517,11 +471,9 @@ int main(int argc, const char **argv) regularImageSubtract(I, Isub_regular, Isub_regular, true); } t = vpTime::measureTimeMs() - t; - std::cout << "Subtract saturation ; t (" << nbIterations - << " iterations)=" << t << " ms" + std::cout << "Subtract saturation ; t (" << nbIterations << " iterations)=" << t << " ms" << " ; Speed-up: " << (t / t_sse) << "X" << std::endl; - std::cout << "(Isub == Isub_regular)? " << (Isub == Isub_regular) - << std::endl; + std::cout << "(Isub == Isub_regular)? " << (Isub == Isub_regular) << std::endl; // Write subtract no saturation filename = vpIoTools::createFilePath(opath, "Klimt_subtract_sat_sse.pgm"); @@ -537,25 +489,19 @@ int main(int argc, const char **argv) vpRect r_crop(0, 0, 411, 507); vpImage I_crop; vpImageTools::crop(I, r_crop, I_crop); - std::cout << "\nI_crop=" << I_crop.getWidth() << "x" - << I_crop.getHeight() << std::endl; + std::cout << "\nI_crop=" << I_crop.getWidth() << "x" << I_crop.getHeight() << std::endl; - vpImage I_invert(I_crop.getHeight(), I_crop.getWidth(), - 255); + vpImage I_invert(I_crop.getHeight(), I_crop.getWidth(), 255); vpImageTools::imageSubtract(I_invert, I_crop, I_invert); - vpImage I_invert_regular(I_crop.getHeight(), - I_crop.getWidth(), 255); + vpImage I_invert_regular(I_crop.getHeight(), I_crop.getWidth(), 255); regularImageSubtract(I_invert_regular, I_crop, I_invert_regular, false); - std::cout << "(I_invert == I_invert_regular)? " - << (I_invert == I_invert_regular) << std::endl; + std::cout << "(I_invert == I_invert_regular)? " << (I_invert == I_invert_regular) << std::endl; - vpImage I_white(I_crop.getHeight(), I_crop.getWidth(), - 255); + vpImage I_white(I_crop.getHeight(), I_crop.getWidth(), 255); vpImage I_invert2 = I_invert; vpImageTools::imageAdd(I_invert2, I_crop, I_invert2); - std::cout << "(I_invert2 == I_white)? " << (I_invert2 == I_white) - << std::endl; + std::cout << "(I_invert2 == I_white)? " << (I_invert2 == I_white) << std::endl; filename = vpIoTools::createFilePath(opath, "Klimt_invert_crop.pgm"); std::cout << "Write: " << filename << std::endl; diff --git a/modules/core/test/image/testImageBinarise.cpp b/modules/core/test/image/testImageBinarise.cpp index 6e2a888507..ff1b6d07d7 100644 --- a/modules/core/test/image/testImageBinarise.cpp +++ b/modules/core/test/image/testImageBinarise.cpp @@ -46,8 +46,7 @@ int main() { - std::cout << "Test vpImageTools::binarise() with different data types." - << std::endl; + 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]; @@ -82,20 +81,17 @@ int main() 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(I_rgba[i][j].R) << " ; " - << static_cast(I_rgba[i][j].G) << " ; " - << static_cast(I_rgba[i][j].B) << " ; " - << static_cast(I_rgba[i][j].A) << std::endl; + std::cout << static_cast(I_rgba[i][j].R) << " ; " << static_cast(I_rgba[i][j].G) << " ; " + << static_cast(I_rgba[i][j].B) << " ; " << static_cast(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, + 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)); + 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++) { @@ -116,10 +112,8 @@ int main() 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(I_rgba[i][j].R) << " ; " - << static_cast(I_rgba[i][j].G) << " ; " - << static_cast(I_rgba[i][j].B) << " ; " - << static_cast(I_rgba[i][j].A) << std::endl; + std::cout << static_cast(I_rgba[i][j].R) << " ; " << static_cast(I_rgba[i][j].G) << " ; " + << static_cast(I_rgba[i][j].B) << " ; " << static_cast(I_rgba[i][j].A) << std::endl; } std::cout << std::endl; } @@ -139,16 +133,13 @@ int main() 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); + 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; + std::cerr << "Results are different between iterate and LUT methods !" << std::endl; return -1; } } @@ -165,27 +156,22 @@ int main() } vpImage I_perf_lut(uchar_array_perf_lut, height, width); - vpImage I_perf_iterate(uchar_array_perf_iterate, height, - width); + vpImage 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); + 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; + 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); + 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 << "LUT: " << t2 << " ms for " << nbIterations << " iterations." << std::endl; std::cout << "\ntestImageBinarise ok !" << std::endl; return 0; diff --git a/modules/core/test/image/testImageComparison.cpp b/modules/core/test/image/testImageComparison.cpp index 2796400352..fd0a22919c 100644 --- a/modules/core/test/image/testImageComparison.cpp +++ b/modules/core/test/image/testImageComparison.cpp @@ -168,8 +168,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -178,13 +177,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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(EXIT_FAILURE); } @@ -199,16 +195,12 @@ int main(int argc, const char **argv) vpImageIo::read(I_Klimt1, filename); vpImageIo::read(I_Klimt2, filename); - std::cout << "\nI_Klimt1=" << I_Klimt1.getWidth() << "x" - << I_Klimt1.getHeight() << std::endl; - std::cout << "I_Klimt2=" << I_Klimt2.getWidth() << "x" - << I_Klimt2.getHeight() << std::endl; + std::cout << "\nI_Klimt1=" << I_Klimt1.getWidth() << "x" << I_Klimt1.getHeight() << std::endl; + std::cout << "I_Klimt2=" << I_Klimt2.getWidth() << "x" << I_Klimt2.getHeight() << std::endl; std::cout << "\nThe two grayscale images are equal." << std::endl; - std::cout << "(I_Klimt1 == I_Klimt2)=" << (I_Klimt1 == I_Klimt2) - << std::endl; - std::cout << "(I_Klimt1 != I_Klimt2)=" << (I_Klimt1 != I_Klimt2) - << std::endl; + std::cout << "(I_Klimt1 == I_Klimt2)=" << (I_Klimt1 == I_Klimt2) << std::endl; + std::cout << "(I_Klimt1 != I_Klimt2)=" << (I_Klimt1 != I_Klimt2) << std::endl; // The two images should be equal if (!(I_Klimt1 == I_Klimt2) || (I_Klimt1 != I_Klimt2)) { @@ -228,10 +220,8 @@ int main(int argc, const char **argv) } std::cout << "\nThe two grayscale images are different." << std::endl; - std::cout << "(I_Klimt1 == I_Klimt2)=" << (I_Klimt1 == I_Klimt2) - << std::endl; - std::cout << "(I_Klimt1 != I_Klimt2)=" << (I_Klimt1 != I_Klimt2) - << std::endl; + std::cout << "(I_Klimt1 == I_Klimt2)=" << (I_Klimt1 == I_Klimt2) << std::endl; + std::cout << "(I_Klimt1 != I_Klimt2)=" << (I_Klimt1 != I_Klimt2) << std::endl; // The two images should be different if ((I_Klimt1 == I_Klimt2) || !(I_Klimt1 != I_Klimt2)) { @@ -250,58 +240,40 @@ int main(int argc, const char **argv) vpImageIo::read(I_color_Klimt1, filename); I_color_Klimt2 = I_color_Klimt1; - std::cout << "\nI_color_Klimt1=" << I_color_Klimt1.getWidth() << "x" - << I_color_Klimt1.getHeight() << std::endl; - std::cout << "I_color_Klimt2=" << I_color_Klimt2.getWidth() << "x" - << I_color_Klimt2.getHeight() << std::endl; + std::cout << "\nI_color_Klimt1=" << I_color_Klimt1.getWidth() << "x" << I_color_Klimt1.getHeight() << std::endl; + std::cout << "I_color_Klimt2=" << I_color_Klimt2.getWidth() << "x" << I_color_Klimt2.getHeight() << std::endl; std::cout << "\nThe two color images are equal." << std::endl; - std::cout << "(I_color_Klimt1 == I_color_Klimt2)=" - << (I_color_Klimt1 == I_color_Klimt2) << std::endl; - std::cout << "(I_color_Klimt1 != I_color_Klimt2)=" - << (I_color_Klimt1 != I_color_Klimt2) << std::endl; + std::cout << "(I_color_Klimt1 == I_color_Klimt2)=" << (I_color_Klimt1 == I_color_Klimt2) << std::endl; + std::cout << "(I_color_Klimt1 != I_color_Klimt2)=" << (I_color_Klimt1 != I_color_Klimt2) << std::endl; // The two images should be equal - if (!(I_color_Klimt1 == I_color_Klimt2) || - (I_color_Klimt1 != I_color_Klimt2)) { + if (!(I_color_Klimt1 == I_color_Klimt2) || (I_color_Klimt1 != I_color_Klimt2)) { std::stringstream ss; ss << "\nProblem when comparing two color images!\n"; - ss << "(I_color_Klimt1 == I_color_Klimt2)=" - << (I_color_Klimt1 == I_color_Klimt2) << std::endl; - ss << "(I_color_Klimt1 != I_color_Klimt2)=" - << (I_color_Klimt1 != I_color_Klimt2) << std::endl; + ss << "(I_color_Klimt1 == I_color_Klimt2)=" << (I_color_Klimt1 == I_color_Klimt2) << std::endl; + ss << "(I_color_Klimt1 != I_color_Klimt2)=" << (I_color_Klimt1 != I_color_Klimt2) << std::endl; throw vpException(vpException::fatalError, ss.str()); } // Modify I_color_Klimt2 - if (I_color_Klimt2[I_color_Klimt2.getHeight() / 2] - [I_color_Klimt2.getWidth() / 2] - .R < 255) { - I_color_Klimt2[I_color_Klimt2.getHeight() / 2] - [I_color_Klimt2.getWidth() / 2] - .R++; + if (I_color_Klimt2[I_color_Klimt2.getHeight() / 2][I_color_Klimt2.getWidth() / 2].R < 255) { + I_color_Klimt2[I_color_Klimt2.getHeight() / 2][I_color_Klimt2.getWidth() / 2].R++; } else { - I_color_Klimt2[I_color_Klimt2.getHeight() / 2] - [I_color_Klimt2.getWidth() / 2] - .R--; + I_color_Klimt2[I_color_Klimt2.getHeight() / 2][I_color_Klimt2.getWidth() / 2].R--; } std::cout << "\nThe two color images are different." << std::endl; - std::cout << "(I_color_Klimt1 == I_color_Klimt2)=" - << (I_color_Klimt1 == I_color_Klimt2) << std::endl; - std::cout << "(I_color_Klimt1 != I_color_Klimt2)=" - << (I_color_Klimt1 != I_color_Klimt2) << std::endl; + std::cout << "(I_color_Klimt1 == I_color_Klimt2)=" << (I_color_Klimt1 == I_color_Klimt2) << std::endl; + std::cout << "(I_color_Klimt1 != I_color_Klimt2)=" << (I_color_Klimt1 != I_color_Klimt2) << std::endl; // The two images should be different - if ((I_color_Klimt1 == I_color_Klimt2) || - !(I_color_Klimt1 != I_color_Klimt2)) { + if ((I_color_Klimt1 == I_color_Klimt2) || !(I_color_Klimt1 != I_color_Klimt2)) { std::stringstream ss; ss << "\nProblem when comparing two color images!\n"; - ss << "(I_color_Klimt1 == I_color_Klimt2)=" - << (I_color_Klimt1 == I_color_Klimt2) << std::endl; - ss << "(I_color_Klimt1 != I_color_Klimt2)=" - << (I_color_Klimt1 != I_color_Klimt2) << std::endl; + ss << "(I_color_Klimt1 == I_color_Klimt2)=" << (I_color_Klimt1 == I_color_Klimt2) << std::endl; + ss << "(I_color_Klimt1 != I_color_Klimt2)=" << (I_color_Klimt1 != I_color_Klimt2) << std::endl; throw vpException(vpException::fatalError, ss.str()); } @@ -311,7 +283,6 @@ int main(int argc, const char **argv) return EXIT_FAILURE; } - std::cout << "\nThe comparison of two images of the same type is OK!" - << std::endl; + std::cout << "\nThe comparison of two images of the same type is OK!" << std::endl; return EXIT_SUCCESS; } diff --git a/modules/core/test/image/testImageFilter.cpp b/modules/core/test/image/testImageFilter.cpp index 10d1319d18..ae05df325f 100644 --- a/modules/core/test/image/testImageFilter.cpp +++ b/modules/core/test/image/testImageFilter.cpp @@ -101,8 +101,7 @@ void usage(const char *name, const char *badparam, std::string ipath) \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) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath) { const char *optarg_; int c; @@ -143,14 +142,12 @@ bool getOptions(int argc, const char **argv, std::string &ipath, } #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408) -bool check_results(const cv::Mat &mat, const vpImage &I, - const unsigned int half_size_y, +bool check_results(const cv::Mat &mat, const vpImage &I, const unsigned int half_size_y, const unsigned int half_size_x) { for (unsigned int i = half_size_y; i < I.getHeight() - half_size_y; i++) { for (unsigned int j = half_size_x; j < I.getWidth() - half_size_x; j++) { - if (!vpMath::equal(mat.at((int)i, (int)j), I[i][j], - std::numeric_limits::epsilon())) { + if (!vpMath::equal(mat.at((int)i, (int)j), I[i][j], std::numeric_limits::epsilon())) { return false; } } @@ -193,8 +190,7 @@ int main(int argc, const char *argv[]) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -276,16 +272,13 @@ int main(int argc, const char *argv[]) std::cout << "\nTest correlation on small image:" << std::endl; std::cout << "(I_correlation_1 == matImg_correlation_1)? " - << check_results(matImg_correlation_1, I_correlation_1, - kernel_1.getRows() / 2, kernel_1.getCols() / 2) + << check_results(matImg_correlation_1, I_correlation_1, kernel_1.getRows() / 2, kernel_1.getCols() / 2) << std::endl; std::cout << "(I_correlation_2 == matImg_correlation_2)? " - << check_results(matImg_correlation_2, I_correlation_2, - kernel_2.getRows() / 2, kernel_2.getCols() / 2) + << check_results(matImg_correlation_2, I_correlation_2, kernel_2.getRows() / 2, kernel_2.getCols() / 2) << std::endl; std::cout << "(I_correlation_3 == matImg_correlation_3)? " - << check_results(matImg_correlation_3, I_correlation_3, - kernel_3.getRows() / 2, kernel_3.getCols() / 2) + << check_results(matImg_correlation_3, I_correlation_3, kernel_3.getRows() / 2, kernel_3.getCols() / 2) << std::endl; #endif @@ -308,35 +301,26 @@ int main(int argc, const char *argv[]) cv::Mat matImg_convolution_1, matImg_convolution_2, matImg_convolution_3; cv::Point anchor1(mat_kernel_1_flip.cols - mat_kernel_1_flip.cols / 2 - 1, - mat_kernel_1_flip.rows - mat_kernel_1_flip.rows / 2 - - 1); - cv::filter2D(matImg, matImg_convolution_1, CV_64F, mat_kernel_1_flip, - anchor1); + mat_kernel_1_flip.rows - mat_kernel_1_flip.rows / 2 - 1); + cv::filter2D(matImg, matImg_convolution_1, CV_64F, mat_kernel_1_flip, anchor1); cv::Point anchor2(mat_kernel_2_flip.cols - mat_kernel_2_flip.cols / 2 - 1, - mat_kernel_2_flip.rows - mat_kernel_2_flip.rows / 2 - - 1); - cv::filter2D(matImg, matImg_convolution_2, CV_64F, mat_kernel_2_flip, - anchor2); + mat_kernel_2_flip.rows - mat_kernel_2_flip.rows / 2 - 1); + cv::filter2D(matImg, matImg_convolution_2, CV_64F, mat_kernel_2_flip, anchor2); cv::Point anchor3(mat_kernel_3_flip.cols - mat_kernel_3_flip.cols / 2 - 1, - mat_kernel_3_flip.rows - mat_kernel_3_flip.rows / 2 - - 1); - cv::filter2D(matImg, matImg_convolution_3, CV_64F, mat_kernel_3_flip, - anchor3); + mat_kernel_3_flip.rows - mat_kernel_3_flip.rows / 2 - 1); + cv::filter2D(matImg, matImg_convolution_3, CV_64F, mat_kernel_3_flip, anchor3); std::cout << "\nTest convolution on small image:" << std::endl; std::cout << "(I_convolution_1 == matImg_convolution_1)? " - << check_results(matImg_convolution_1, I_convolution_1, - kernel_1.getRows() / 2, kernel_1.getCols() / 2) + << check_results(matImg_convolution_1, I_convolution_1, kernel_1.getRows() / 2, kernel_1.getCols() / 2) << std::endl; std::cout << "(I_convolution_2 == matImg_convolution_2)? " - << check_results(matImg_convolution_2, I_convolution_2, - kernel_2.getRows() / 2, kernel_2.getCols() / 2) + << check_results(matImg_convolution_2, I_convolution_2, kernel_2.getRows() / 2, kernel_2.getCols() / 2) << std::endl; std::cout << "(I_convolution_3 == matImg_convolution_3)? " - << check_results(matImg_convolution_3, I_convolution_3, - kernel_3.getRows() / 2, kernel_3.getCols() / 2) + << check_results(matImg_convolution_3, I_convolution_3, kernel_3.getRows() / 2, kernel_3.getCols() / 2) << std::endl; #endif @@ -356,8 +340,7 @@ int main(int argc, const char *argv[]) vpImageFilter::filter(I, I_correlation_2, kernel_2); vpImageFilter::filter(I, I_correlation_3, kernel_3); t = vpTime::measureTimeMs() - t; - std::cout << "\nTime to do 3 correlation filtering: " << t - << " ms ; Mean: " << t / 3.0 << " ms" << std::endl; + std::cout << "\nTime to do 3 correlation filtering: " << t << " ms ; Mean: " << t / 3.0 << " ms" << std::endl; #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408) vpImageConvert::convert(I, matImg); @@ -367,37 +350,27 @@ int main(int argc, const char *argv[]) cv::filter2D(matImg, matImg_correlation_2, CV_64F, mat_kernel_2); cv::filter2D(matImg, matImg_correlation_3, CV_64F, mat_kernel_3); t = vpTime::measureTimeMs() - t; - std::cout << "Time to do 3 cv::filter2D: " << t - << " ms ; Mean: " << t / 3.0 << " ms" << std::endl; + std::cout << "Time to do 3 cv::filter2D: " << t << " ms ; Mean: " << t / 3.0 << " ms" << std::endl; std::cout << "\nTest correlation on Klimt image:" << std::endl; - bool test = check_results(matImg_correlation_1, I_correlation_1, - kernel_1.getRows() / 2, kernel_1.getCols() / 2); - std::cout << "(I_correlation_1 == matImg_correlation_1)? " << test - << std::endl; + bool test = check_results(matImg_correlation_1, I_correlation_1, kernel_1.getRows() / 2, kernel_1.getCols() / 2); + std::cout << "(I_correlation_1 == matImg_correlation_1)? " << test << std::endl; if (!test) { - std::cerr << "Failed test1 correlation with vpImageFilter::filter()!" - << std::endl; + std::cerr << "Failed test1 correlation with vpImageFilter::filter()!" << std::endl; return EXIT_FAILURE; } - test = check_results(matImg_correlation_2, I_correlation_2, - kernel_2.getRows() / 2, kernel_2.getCols() / 2); - std::cout << "(I_correlation_2 == matImg_correlation_2)? " << test - << std::endl; + test = check_results(matImg_correlation_2, I_correlation_2, kernel_2.getRows() / 2, kernel_2.getCols() / 2); + std::cout << "(I_correlation_2 == matImg_correlation_2)? " << test << std::endl; if (!test) { - std::cerr << "Failed test2 correlation with vpImageFilter::filter()!" - << std::endl; + std::cerr << "Failed test2 correlation with vpImageFilter::filter()!" << std::endl; return EXIT_FAILURE; } - test = check_results(matImg_correlation_3, I_correlation_3, - kernel_3.getRows() / 2, kernel_3.getCols() / 2); - std::cout << "(I_correlation_3 == matImg_correlation_3)? " << test - << std::endl; + test = check_results(matImg_correlation_3, I_correlation_3, kernel_3.getRows() / 2, kernel_3.getCols() / 2); + std::cout << "(I_correlation_3 == matImg_correlation_3)? " << test << std::endl; if (!test) { - std::cerr << "Failed test3 correlation with vpImageFilter::filter()!" - << std::endl; + std::cerr << "Failed test3 correlation with vpImageFilter::filter()!" << std::endl; return EXIT_FAILURE; } #endif @@ -408,50 +381,36 @@ int main(int argc, const char *argv[]) vpImageFilter::filter(I, I_convolution_2, kernel_2, true); vpImageFilter::filter(I, I_convolution_3, kernel_3, true); t = vpTime::measureTimeMs() - t; - std::cout << "\nTime to do 3 convolution filtering: " << t - << " ms ; Mean: " << t / 3.0 << " ms" << std::endl; + std::cout << "\nTime to do 3 convolution filtering: " << t << " ms ; Mean: " << t / 3.0 << " ms" << std::endl; #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408) t = vpTime::measureTimeMs(); - cv::filter2D(matImg, matImg_convolution_1, CV_64F, mat_kernel_1_flip, - anchor1); - cv::filter2D(matImg, matImg_convolution_2, CV_64F, mat_kernel_2_flip, - anchor2); - cv::filter2D(matImg, matImg_convolution_3, CV_64F, mat_kernel_3_flip, - anchor3); + cv::filter2D(matImg, matImg_convolution_1, CV_64F, mat_kernel_1_flip, anchor1); + cv::filter2D(matImg, matImg_convolution_2, CV_64F, mat_kernel_2_flip, anchor2); + cv::filter2D(matImg, matImg_convolution_3, CV_64F, mat_kernel_3_flip, anchor3); t = vpTime::measureTimeMs() - t; - std::cout << "Time to do 3 cv::filter2D: " << t - << " ms ; Mean: " << t / 3.0 << " ms" << std::endl; + std::cout << "Time to do 3 cv::filter2D: " << t << " ms ; Mean: " << t / 3.0 << " ms" << std::endl; std::cout << "\nTest convolution on Klimt image:" << std::endl; - test = check_results(matImg_convolution_1, I_convolution_1, - kernel_1.getRows() / 2, kernel_1.getCols() / 2); - std::cout << "(I_convolution_1 == matImg_convolution_1)? " << test - << std::endl; + test = check_results(matImg_convolution_1, I_convolution_1, kernel_1.getRows() / 2, kernel_1.getCols() / 2); + std::cout << "(I_convolution_1 == matImg_convolution_1)? " << test << std::endl; if (!test) { - std::cerr << "Failed test1 convolution with vpImageFilter::filter()!" - << std::endl; + std::cerr << "Failed test1 convolution with vpImageFilter::filter()!" << std::endl; return EXIT_FAILURE; } - test = check_results(matImg_convolution_2, I_convolution_2, - kernel_2.getRows() / 2, kernel_2.getCols() / 2); - std::cout << "(I_convolution_2 == matImg_convolution_2)? " << test - << std::endl; + test = check_results(matImg_convolution_2, I_convolution_2, kernel_2.getRows() / 2, kernel_2.getCols() / 2); + std::cout << "(I_convolution_2 == matImg_convolution_2)? " << test << std::endl; if (!test) { - std::cerr << "Failed test2 convolution with vpImageFilter::filter()!" - << std::endl; + std::cerr << "Failed test2 convolution with vpImageFilter::filter()!" << std::endl; return EXIT_FAILURE; } - test = check_results(matImg_convolution_3, I_convolution_3, - kernel_3.getRows() / 2, kernel_3.getCols() / 2); - std::cout << "(I_convolution_3 == matImg_convolution_3)? " << test - << std::endl; + test = check_results(matImg_convolution_3, I_convolution_3, kernel_3.getRows() / 2, kernel_3.getCols() / 2); + std::cout << "(I_convolution_3 == matImg_convolution_3)? " << test << std::endl; if (!test) { - std::cerr << "Failed test3 convolution with vpImageFilter::filter()!" - << std::endl; + std::cerr << "Failed test3 convolution with vpImageFilter::filter()!" << std::endl; return EXIT_FAILURE; } #endif @@ -499,9 +458,7 @@ int main(int argc, const char *argv[]) std::cout << "\nTest Sobel on Klimt image:" << std::endl; std::cout << "(I_sobel_x == matImg_sobel_x)? " - << check_results(matImg_sobel_x, I_sobel_x, - kernel_sobel_x.getRows() / 2, - kernel_sobel_x.getCols() / 2) + << check_results(matImg_sobel_x, I_sobel_x, kernel_sobel_x.getRows() / 2, kernel_sobel_x.getCols() / 2) << std::endl; #endif @@ -517,14 +474,10 @@ int main(int argc, const char *argv[]) cv::Sobel(matImg, matImg_sobel_y, CV_64F, 0, 1, 5); std::cout << "(Iu == matImg_sobel_x)? " - << check_results(matImg_sobel_x, Iu, - kernel_sobel_x.getRows() / 2, - kernel_sobel_x.getCols() / 2) + << check_results(matImg_sobel_x, Iu, kernel_sobel_x.getRows() / 2, kernel_sobel_x.getCols() / 2) << std::endl; std::cout << "(Iv == matImg_sobel_y)? " - << check_results(matImg_sobel_y, Iv, - kernel_sobel_x.getRows() / 2, - kernel_sobel_x.getCols() / 2) + << check_results(matImg_sobel_y, Iv, kernel_sobel_x.getRows() / 2, kernel_sobel_x.getCols() / 2) << std::endl; #endif @@ -549,8 +502,7 @@ int main(int argc, const char *argv[]) std::cout << "\nTime to do sepFilter: " << t << " ms" << std::endl; #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408) - test = check_results(matImg_sobel_x, Iu, I_sep_filtered.getRows() / 2, - kernel_sobel_x.getCols() / 2); + test = check_results(matImg_sobel_x, Iu, I_sep_filtered.getRows() / 2, kernel_sobel_x.getCols() / 2); std::cout << "(I_sep_filtered == matImg_sobel_x)? " << test << std::endl; if (!test) { diff --git a/modules/core/test/image/testImageMorphology.cpp b/modules/core/test/image/testImageMorphology.cpp index 1b7a6cbbd9..544fd3cc33 100644 --- a/modules/core/test/image/testImageMorphology.cpp +++ b/modules/core/test/image/testImageMorphology.cpp @@ -58,8 +58,7 @@ \param ipath: Input image path. \param nbiter : Iteration number. */ -void usage(const char *name, const char *badparam, std::string ipath, - int nbiter) +void usage(const char *name, const char *badparam, std::string ipath, int nbiter) { fprintf(stdout, "\n\ Test vpImageMorphology functions.\n\ @@ -97,8 +96,7 @@ OPTIONS: Default\n\ \param nbIterations : Number of benchmark iterations. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - int &nbIterations) +bool getOptions(int argc, const char **argv, std::string &ipath, int &nbIterations) { const char *optarg_; int c; @@ -151,8 +149,7 @@ void printMatrix(const vpImage &I, const std::string &name) // Erosion in the general case on grayscale images void generalErosion(vpImage &I, - vpImageMorphology::vpConnexityType connexity = - vpImageMorphology::CONNEXITY_4) + vpImageMorphology::vpConnexityType connexity = vpImageMorphology::CONNEXITY_4) { if (I.getSize() == 0) { std::cerr << "Input image is empty!" << std::endl; @@ -176,8 +173,7 @@ void generalErosion(vpImage &I, } if (connexity == vpImageMorphology::CONNEXITY_4) { - unsigned int offset[5] = {1, J.getWidth(), J.getWidth() + 1, - J.getWidth() + 2, J.getWidth() * 2 + 1}; + unsigned int offset[5] = {1, J.getWidth(), J.getWidth() + 1, J.getWidth() + 2, J.getWidth() * 2 + 1}; for (unsigned int i = 0; i < I.getHeight(); i++) { unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth(); @@ -222,8 +218,7 @@ void generalErosion(vpImage &I, // Dilatation in the general case on grayscale images void generalDilatation(vpImage &I, - vpImageMorphology::vpConnexityType connexity = - vpImageMorphology::CONNEXITY_4) + vpImageMorphology::vpConnexityType connexity = vpImageMorphology::CONNEXITY_4) { if (I.getSize() == 0) { std::cerr << "Input image is empty!" << std::endl; @@ -247,8 +242,7 @@ void generalDilatation(vpImage &I, } if (connexity == vpImageMorphology::CONNEXITY_4) { - unsigned int offset[5] = {1, J.getWidth(), J.getWidth() + 1, - J.getWidth() + 2, J.getWidth() * 2 + 1}; + unsigned int offset[5] = {1, J.getWidth(), J.getWidth() + 1, J.getWidth() + 2, J.getWidth() * 2 + 1}; for (unsigned int i = 0; i < I.getHeight(); i++) { unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth(); @@ -346,8 +340,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -356,24 +349,19 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, nbIterations); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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(EXIT_FAILURE); } // Create a binary image - unsigned char image_data[8 * 16] = { - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 0, 0, - 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, - 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, - 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, - 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0}; + unsigned char image_data[8 * 16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, + 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, + 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, + 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0}; vpImage I(image_data, 8, 16, true); printMatrix(I, "I"); @@ -383,12 +371,8 @@ int main(int argc, const char **argv) vpImage I_dilatation1 = I; vpImage I_dilatation2 = I; - vpImageMorphology::dilatation(I_dilatation1, (unsigned char)1, - (unsigned char)0, - vpImageMorphology::CONNEXITY_4); - vpImageMorphology::dilatation(I_dilatation2, (unsigned char)1, - (unsigned char)0, - vpImageMorphology::CONNEXITY_8); + vpImageMorphology::dilatation(I_dilatation1, (unsigned char)1, (unsigned char)0, vpImageMorphology::CONNEXITY_4); + vpImageMorphology::dilatation(I_dilatation2, (unsigned char)1, (unsigned char)0, vpImageMorphology::CONNEXITY_8); printMatrix(I_dilatation1, "I_dilatation1"); printMatrix(I_dilatation2, "I_dilatation2"); @@ -397,10 +381,8 @@ int main(int argc, const char **argv) vpImage I_erosion1 = I_dilatation1; vpImage I_erosion2 = I_dilatation2; - vpImageMorphology::erosion(I_erosion1, (unsigned char)1, (unsigned char)0, - vpImageMorphology::CONNEXITY_4); - vpImageMorphology::erosion(I_erosion2, (unsigned char)1, (unsigned char)0, - vpImageMorphology::CONNEXITY_8); + vpImageMorphology::erosion(I_erosion1, (unsigned char)1, (unsigned char)0, vpImageMorphology::CONNEXITY_4); + vpImageMorphology::erosion(I_erosion2, (unsigned char)1, (unsigned char)0, vpImageMorphology::CONNEXITY_8); printMatrix(I_erosion1, "I_erosion1"); printMatrix(I_erosion2, "I_erosion2"); @@ -410,113 +392,89 @@ int main(int argc, const char **argv) vpImage I_dilatation1_sse = I; vpImage I_dilatation2_sse = I; - vpImageMorphology::dilatation(I_dilatation1_sse, - vpImageMorphology::CONNEXITY_4); - vpImageMorphology::dilatation(I_dilatation2_sse, - vpImageMorphology::CONNEXITY_8); + vpImageMorphology::dilatation(I_dilatation1_sse, vpImageMorphology::CONNEXITY_4); + vpImageMorphology::dilatation(I_dilatation2_sse, vpImageMorphology::CONNEXITY_8); printMatrix(I_dilatation1_sse, "I_dilatation1_sse"); printMatrix(I_dilatation2_sse, "I_dilatation2_sse"); - std::cout << "\n(I_dilatation1 == I_dilatation1_sse)? " - << (I_dilatation1 == I_dilatation1_sse) << std::endl; - std::cout << "(I_dilatation2 == I_dilatation2_sse)? " - << (I_dilatation2 == I_dilatation2_sse) << std::endl; + std::cout << "\n(I_dilatation1 == I_dilatation1_sse)? " << (I_dilatation1 == I_dilatation1_sse) << std::endl; + std::cout << "(I_dilatation2 == I_dilatation2_sse)? " << (I_dilatation2 == I_dilatation2_sse) << std::endl; if ((I_dilatation1 != I_dilatation1_sse)) { - throw vpException(vpException::fatalError, - "(I_dilatation1 != I_dilatation1_sse)"); + throw vpException(vpException::fatalError, "(I_dilatation1 != I_dilatation1_sse)"); } if ((I_dilatation2 != I_dilatation2_sse)) { - throw vpException(vpException::fatalError, - "(I_dilatation2 != I_dilatation2_sse)"); + throw vpException(vpException::fatalError, "(I_dilatation2 != I_dilatation2_sse)"); } // Erosion vpImage I_erosion1_sse = I_dilatation1_sse; vpImage I_erosion2_sse = I_dilatation2_sse; - vpImageMorphology::erosion(I_erosion1_sse, - vpImageMorphology::CONNEXITY_4); - vpImageMorphology::erosion(I_erosion2_sse, - vpImageMorphology::CONNEXITY_8); + vpImageMorphology::erosion(I_erosion1_sse, vpImageMorphology::CONNEXITY_4); + vpImageMorphology::erosion(I_erosion2_sse, vpImageMorphology::CONNEXITY_8); printMatrix(I_erosion1_sse, "I_erosion1_sse"); printMatrix(I_erosion2_sse, "I_erosion2_sse"); - std::cout << "\n(I_erosion1 == I_erosion1_sse)? " - << (I_erosion1 == I_erosion1_sse) << std::endl; - std::cout << "(I_erosion2 == I_erosion2_sse)? " - << (I_erosion2 == I_erosion2_sse) << std::endl; + std::cout << "\n(I_erosion1 == I_erosion1_sse)? " << (I_erosion1 == I_erosion1_sse) << std::endl; + std::cout << "(I_erosion2 == I_erosion2_sse)? " << (I_erosion2 == I_erosion2_sse) << std::endl; if ((I_erosion1 != I_erosion1_sse)) { - throw vpException(vpException::fatalError, - "(I_erosion1 != I_erosion1_sse)"); + throw vpException(vpException::fatalError, "(I_erosion1 != I_erosion1_sse)"); } if ((I_erosion2 != I_erosion2_sse)) { - throw vpException(vpException::fatalError, - "(I_erosion2 != I_erosion2_sse)"); + throw vpException(vpException::fatalError, "(I_erosion2 != I_erosion2_sse)"); } // Check results against Matlab unsigned char image_data_dilated1[8 * 16] = { - 0, 0, 0, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 1, - 1, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, - 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 0, - 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, - 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, - 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1}; + 0, 0, 0, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 0, + 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, + 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, + 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1}; vpImage I_check_dilated1(image_data_dilated1, 8, 16, true); if (I_check_dilated1 != I_dilatation1_sse) { - throw vpException(vpException::fatalError, - "(I_check_dilated1 != I_dilatation1_sse)"); + throw vpException(vpException::fatalError, "(I_check_dilated1 != I_dilatation1_sse)"); } unsigned char image_data_dilated2[8 * 16] = { - 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, - 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, - 1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, - 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1}; + 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, + 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, + 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1}; vpImage I_check_dilated2(image_data_dilated2, 8, 16, true); if (I_check_dilated2 != I_dilatation2_sse) { - throw vpException(vpException::fatalError, - "(I_check_dilated2 != I_dilatation2_sse)"); + throw vpException(vpException::fatalError, "(I_check_dilated2 != I_dilatation2_sse)"); } unsigned char image_data_eroded1[8 * 16] = { - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 1, 0, 0, - 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, - 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, - 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, - 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0}; + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 1, 1, 1, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, + 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0}; vpImage I_check_eroded1(image_data_eroded1, 8, 16, true); if (I_check_eroded1 != I_erosion1_sse) { - throw vpException(vpException::fatalError, - "(I_check_eroded1 != I_erosion1_sse)"); + throw vpException(vpException::fatalError, "(I_check_eroded1 != I_erosion1_sse)"); } unsigned char image_data_eroded2[8 * 16] = { - 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, - 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, - 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 0, - 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, - 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, - 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1}; + 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, + 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, + 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1}; vpImage I_check_eroded2(image_data_eroded2, 8, 16, true); if (I_check_eroded2 != I_erosion2_sse) { - throw vpException(vpException::fatalError, - "(I_check_eroded2 != I_erosion2_sse)"); + throw vpException(vpException::fatalError, "(I_check_eroded2 != I_erosion2_sse)"); } // Morphology on grayscale images @@ -528,191 +486,135 @@ int main(int argc, const char **argv) vpImage I_magic_square_dilatation1 = I_magic_square; vpImage I_magic_square_dilatation1_sse = I_magic_square; - generalDilatation(I_magic_square_dilatation1, - vpImageMorphology::CONNEXITY_4); - vpImageMorphology::dilatation(I_magic_square_dilatation1_sse, - vpImageMorphology::CONNEXITY_4); + generalDilatation(I_magic_square_dilatation1, vpImageMorphology::CONNEXITY_4); + vpImageMorphology::dilatation(I_magic_square_dilatation1_sse, vpImageMorphology::CONNEXITY_4); if ((I_magic_square_dilatation1 != I_magic_square_dilatation1_sse)) { - throw vpException( - vpException::fatalError, - "(I_magic_square_dilatation1 != I_magic_square_dilatation1_sse)"); + throw vpException(vpException::fatalError, "(I_magic_square_dilatation1 != I_magic_square_dilatation1_sse)"); } // Dilatation CONNEXITY_8 grayscale vpImage I_magic_square_dilatation2 = I_magic_square; vpImage I_magic_square_dilatation2_sse = I_magic_square; - generalDilatation(I_magic_square_dilatation2, - vpImageMorphology::CONNEXITY_8); - vpImageMorphology::dilatation(I_magic_square_dilatation2_sse, - vpImageMorphology::CONNEXITY_8); + generalDilatation(I_magic_square_dilatation2, vpImageMorphology::CONNEXITY_8); + vpImageMorphology::dilatation(I_magic_square_dilatation2_sse, vpImageMorphology::CONNEXITY_8); if ((I_magic_square_dilatation2 != I_magic_square_dilatation2_sse)) { - throw vpException( - vpException::fatalError, - "(I_magic_square_dilatation2 != I_magic_square_dilatation2_sse)"); + throw vpException(vpException::fatalError, "(I_magic_square_dilatation2 != I_magic_square_dilatation2_sse)"); } // Erosion CONNEXITY_4 grayscale - vpImage I_magic_square_erosion1 = - I_magic_square_dilatation1; - vpImage I_magic_square_erosion1_sse = - I_magic_square_dilatation1_sse; + vpImage I_magic_square_erosion1 = I_magic_square_dilatation1; + vpImage I_magic_square_erosion1_sse = I_magic_square_dilatation1_sse; generalErosion(I_magic_square_erosion1, vpImageMorphology::CONNEXITY_4); - vpImageMorphology::erosion(I_magic_square_erosion1_sse, - vpImageMorphology::CONNEXITY_4); + vpImageMorphology::erosion(I_magic_square_erosion1_sse, vpImageMorphology::CONNEXITY_4); if ((I_magic_square_erosion1 != I_magic_square_erosion1_sse)) { - throw vpException( - vpException::fatalError, - "(I_magic_square_erosion1 != I_magic_square_erosion1_sse)"); + throw vpException(vpException::fatalError, "(I_magic_square_erosion1 != I_magic_square_erosion1_sse)"); } // Dilatation CONNEXITY_8 grayscale - vpImage I_magic_square_erosion2 = - I_magic_square_dilatation2; - vpImage I_magic_square_erosion2_sse = - I_magic_square_dilatation2_sse; + vpImage I_magic_square_erosion2 = I_magic_square_dilatation2; + vpImage I_magic_square_erosion2_sse = I_magic_square_dilatation2_sse; generalErosion(I_magic_square_erosion2, vpImageMorphology::CONNEXITY_8); - vpImageMorphology::erosion(I_magic_square_erosion2_sse, - vpImageMorphology::CONNEXITY_8); + vpImageMorphology::erosion(I_magic_square_erosion2_sse, vpImageMorphology::CONNEXITY_8); if ((I_magic_square_erosion2 != I_magic_square_erosion2_sse)) { - throw vpException( - vpException::fatalError, - "(I_magic_square_erosion2 != I_magic_square_erosion2_sse)"); + throw vpException(vpException::fatalError, "(I_magic_square_erosion2 != I_magic_square_erosion2_sse)"); } // Check results against Matlab (grayscale) unsigned char image_data2_dilated1[17 * 17] = { - 174, 193, 212, 231, 250, 255, 255, 255, 255, 39, 58, 77, 96, 115, - 134, 153, 154, 192, 211, 230, 249, 255, 255, 255, 255, 38, 57, 76, - 95, 114, 133, 152, 170, 172, 210, 229, 248, 255, 255, 255, 255, 37, - 56, 75, 94, 113, 132, 151, 170, 172, 190, 228, 247, 255, 255, 255, - 255, 36, 55, 74, 93, 112, 131, 150, 169, 187, 190, 208, 246, 255, - 255, 255, 255, 51, 54, 73, 92, 111, 130, 149, 168, 187, 189, 208, - 226, 255, 255, 255, 255, 51, 53, 72, 91, 110, 129, 148, 167, 186, - 204, 207, 226, 244, 255, 255, 255, 50, 68, 71, 90, 109, 128, 147, - 166, 185, 204, 206, 225, 244, 255, 255, 255, 49, 68, 70, 89, 108, - 127, 146, 165, 184, 203, 221, 224, 243, 255, 255, 255, 48, 67, 85, - 88, 107, 126, 145, 164, 183, 202, 221, 223, 242, 255, 255, 255, 47, - 66, 85, 87, 106, 125, 144, 163, 182, 201, 220, 238, 241, 255, 255, - 255, 255, 65, 84, 102, 105, 124, 143, 162, 181, 200, 219, 238, 240, - 255, 255, 255, 255, 45, 83, 102, 104, 123, 142, 161, 180, 199, 218, - 237, 255, 255, 255, 255, 255, 45, 63, 101, 119, 122, 141, 160, 179, - 198, 217, 236, 255, 255, 255, 255, 255, 44, 63, 81, 119, 121, 140, - 159, 178, 197, 216, 235, 254, 255, 255, 255, 255, 43, 62, 81, 99, - 136, 139, 158, 177, 196, 215, 234, 253, 255, 255, 255, 255, 42, 61, - 80, 99, 117, 138, 157, 176, 195, 214, 233, 252, 255, 255, 255, 255, - 41, 60, 79, 98, 117, 135, 156, 175, 194, 213, 232, 251, 255, 255, - 255, 255, 40, 59, 78, 97, 116, 135, 135}; - - vpImage I2_check_dilated1(image_data2_dilated1, 17, 17, - true); + 174, 193, 212, 231, 250, 255, 255, 255, 255, 39, 58, 77, 96, 115, 134, 153, 154, 192, 211, 230, 249, + 255, 255, 255, 255, 38, 57, 76, 95, 114, 133, 152, 170, 172, 210, 229, 248, 255, 255, 255, 255, 37, + 56, 75, 94, 113, 132, 151, 170, 172, 190, 228, 247, 255, 255, 255, 255, 36, 55, 74, 93, 112, 131, + 150, 169, 187, 190, 208, 246, 255, 255, 255, 255, 51, 54, 73, 92, 111, 130, 149, 168, 187, 189, 208, + 226, 255, 255, 255, 255, 51, 53, 72, 91, 110, 129, 148, 167, 186, 204, 207, 226, 244, 255, 255, 255, + 50, 68, 71, 90, 109, 128, 147, 166, 185, 204, 206, 225, 244, 255, 255, 255, 49, 68, 70, 89, 108, + 127, 146, 165, 184, 203, 221, 224, 243, 255, 255, 255, 48, 67, 85, 88, 107, 126, 145, 164, 183, 202, + 221, 223, 242, 255, 255, 255, 47, 66, 85, 87, 106, 125, 144, 163, 182, 201, 220, 238, 241, 255, 255, + 255, 255, 65, 84, 102, 105, 124, 143, 162, 181, 200, 219, 238, 240, 255, 255, 255, 255, 45, 83, 102, + 104, 123, 142, 161, 180, 199, 218, 237, 255, 255, 255, 255, 255, 45, 63, 101, 119, 122, 141, 160, 179, + 198, 217, 236, 255, 255, 255, 255, 255, 44, 63, 81, 119, 121, 140, 159, 178, 197, 216, 235, 254, 255, + 255, 255, 255, 43, 62, 81, 99, 136, 139, 158, 177, 196, 215, 234, 253, 255, 255, 255, 255, 42, 61, + 80, 99, 117, 138, 157, 176, 195, 214, 233, 252, 255, 255, 255, 255, 41, 60, 79, 98, 117, 135, 156, + 175, 194, 213, 232, 251, 255, 255, 255, 255, 40, 59, 78, 97, 116, 135, 135}; + + vpImage I2_check_dilated1(image_data2_dilated1, 17, 17, true); if (I2_check_dilated1 != I_magic_square_dilatation1_sse) { - throw vpException( - vpException::fatalError, - "(I2_check_dilated1 != I_magic_square_dilatation1_sse)"); + throw vpException(vpException::fatalError, "(I2_check_dilated1 != I_magic_square_dilatation1_sse)"); } unsigned char image_data2_dilated2[17 * 17] = { - 192, 211, 230, 249, 255, 255, 255, 255, 255, 57, 76, 95, 114, 133, - 152, 154, 154, 210, 229, 248, 255, 255, 255, 255, 255, 255, 75, 94, - 113, 132, 151, 170, 172, 172, 228, 247, 255, 255, 255, 255, 255, 255, - 74, 93, 112, 131, 150, 169, 171, 190, 190, 246, 255, 255, 255, 255, - 255, 255, 73, 92, 111, 130, 149, 168, 187, 189, 208, 208, 255, 255, - 255, 255, 255, 255, 72, 91, 110, 129, 148, 167, 186, 188, 207, 226, - 226, 255, 255, 255, 255, 255, 71, 90, 109, 128, 147, 166, 185, 204, - 206, 225, 244, 244, 255, 255, 255, 255, 70, 89, 108, 127, 146, 165, - 184, 203, 205, 224, 243, 255, 255, 255, 255, 255, 69, 88, 107, 126, - 145, 164, 183, 202, 221, 223, 242, 255, 255, 255, 255, 255, 85, 87, - 106, 125, 144, 163, 182, 201, 220, 222, 241, 255, 255, 255, 255, 65, - 84, 86, 105, 124, 143, 162, 181, 200, 219, 238, 240, 255, 255, 255, - 255, 255, 83, 102, 104, 123, 142, 161, 180, 199, 218, 237, 239, 255, - 255, 255, 255, 255, 255, 101, 103, 122, 141, 160, 179, 198, 217, 236, - 255, 255, 255, 255, 255, 255, 255, 63, 119, 121, 140, 159, 178, 197, - 216, 235, 254, 255, 255, 255, 255, 255, 255, 81, 81, 120, 139, 158, - 177, 196, 215, 234, 253, 255, 255, 255, 255, 255, 255, 80, 99, 99, - 138, 157, 176, 195, 214, 233, 252, 255, 255, 255, 255, 255, 255, 79, - 98, 117, 117, 156, 175, 194, 213, 232, 251, 255, 255, 255, 255, 255, - 255, 78, 97, 116, 135, 135, 156, 175, 194, 213, 232, 251, 255, 255, - 255, 255, 255, 59, 78, 97, 116, 135, 135}; - - vpImage I2_check_dilated2(image_data2_dilated2, 17, 17, - true); + 192, 211, 230, 249, 255, 255, 255, 255, 255, 57, 76, 95, 114, 133, 152, 154, 154, 210, 229, 248, 255, + 255, 255, 255, 255, 255, 75, 94, 113, 132, 151, 170, 172, 172, 228, 247, 255, 255, 255, 255, 255, 255, + 74, 93, 112, 131, 150, 169, 171, 190, 190, 246, 255, 255, 255, 255, 255, 255, 73, 92, 111, 130, 149, + 168, 187, 189, 208, 208, 255, 255, 255, 255, 255, 255, 72, 91, 110, 129, 148, 167, 186, 188, 207, 226, + 226, 255, 255, 255, 255, 255, 71, 90, 109, 128, 147, 166, 185, 204, 206, 225, 244, 244, 255, 255, 255, + 255, 70, 89, 108, 127, 146, 165, 184, 203, 205, 224, 243, 255, 255, 255, 255, 255, 69, 88, 107, 126, + 145, 164, 183, 202, 221, 223, 242, 255, 255, 255, 255, 255, 85, 87, 106, 125, 144, 163, 182, 201, 220, + 222, 241, 255, 255, 255, 255, 65, 84, 86, 105, 124, 143, 162, 181, 200, 219, 238, 240, 255, 255, 255, + 255, 255, 83, 102, 104, 123, 142, 161, 180, 199, 218, 237, 239, 255, 255, 255, 255, 255, 255, 101, 103, + 122, 141, 160, 179, 198, 217, 236, 255, 255, 255, 255, 255, 255, 255, 63, 119, 121, 140, 159, 178, 197, + 216, 235, 254, 255, 255, 255, 255, 255, 255, 81, 81, 120, 139, 158, 177, 196, 215, 234, 253, 255, 255, + 255, 255, 255, 255, 80, 99, 99, 138, 157, 176, 195, 214, 233, 252, 255, 255, 255, 255, 255, 255, 79, + 98, 117, 117, 156, 175, 194, 213, 232, 251, 255, 255, 255, 255, 255, 255, 78, 97, 116, 135, 135, 156, + 175, 194, 213, 232, 251, 255, 255, 255, 255, 255, 59, 78, 97, 116, 135, 135}; + + vpImage I2_check_dilated2(image_data2_dilated2, 17, 17, true); if (I2_check_dilated2 != I_magic_square_dilatation2_sse) { - throw vpException( - vpException::fatalError, - "(I2_check_dilated2 != I_magic_square_dilatation2_sse)"); + throw vpException(vpException::fatalError, "(I2_check_dilated2 != I_magic_square_dilatation2_sse)"); } unsigned char image_data2_eroded1[17 * 17] = { - 174, 174, 193, 212, 231, 250, 255, 255, 38, 39, 39, 58, 77, 96, - 115, 134, 153, 174, 192, 211, 230, 249, 255, 255, 37, 38, 38, 57, - 76, 95, 114, 133, 152, 154, 192, 210, 229, 248, 255, 255, 36, 37, - 37, 56, 75, 94, 113, 132, 151, 170, 172, 210, 228, 247, 255, 255, - 36, 36, 36, 55, 74, 93, 112, 131, 150, 169, 172, 190, 228, 246, - 255, 255, 51, 51, 36, 54, 73, 92, 111, 130, 149, 168, 187, 189, - 208, 246, 255, 255, 50, 51, 51, 53, 72, 91, 110, 129, 148, 167, - 186, 189, 207, 226, 255, 255, 49, 50, 50, 53, 71, 90, 109, 128, - 147, 166, 185, 204, 206, 225, 244, 255, 48, 49, 49, 68, 70, 89, - 108, 127, 146, 165, 184, 203, 206, 224, 243, 255, 47, 48, 48, 67, - 70, 88, 107, 126, 145, 164, 183, 202, 221, 223, 242, 255, 255, 47, - 47, 66, 85, 87, 106, 125, 144, 163, 182, 201, 220, 223, 241, 255, - 255, 45, 47, 65, 84, 87, 105, 124, 143, 162, 181, 200, 219, 238, - 240, 255, 255, 45, 45, 65, 83, 102, 104, 123, 142, 161, 180, 199, - 218, 237, 240, 255, 255, 44, 45, 45, 83, 101, 104, 122, 141, 160, - 179, 198, 217, 236, 255, 255, 255, 43, 44, 44, 63, 101, 119, 121, - 140, 159, 178, 197, 216, 235, 254, 255, 255, 42, 43, 43, 62, 81, - 119, 121, 139, 158, 177, 196, 215, 234, 253, 255, 255, 41, 42, 42, - 61, 80, 99, 136, 138, 157, 176, 195, 214, 233, 252, 255, 255, 40, - 41, 41, 60, 79, 98, 117, 138, 156, 175, 194, 213, 232, 251, 255, - 255, 40, 40, 40, 59, 78, 97, 116, 135}; - - vpImage I2_check_eroded1(image_data2_eroded1, 17, 17, - true); + 174, 174, 193, 212, 231, 250, 255, 255, 38, 39, 39, 58, 77, 96, 115, 134, 153, 174, 192, 211, 230, + 249, 255, 255, 37, 38, 38, 57, 76, 95, 114, 133, 152, 154, 192, 210, 229, 248, 255, 255, 36, 37, + 37, 56, 75, 94, 113, 132, 151, 170, 172, 210, 228, 247, 255, 255, 36, 36, 36, 55, 74, 93, 112, + 131, 150, 169, 172, 190, 228, 246, 255, 255, 51, 51, 36, 54, 73, 92, 111, 130, 149, 168, 187, 189, + 208, 246, 255, 255, 50, 51, 51, 53, 72, 91, 110, 129, 148, 167, 186, 189, 207, 226, 255, 255, 49, + 50, 50, 53, 71, 90, 109, 128, 147, 166, 185, 204, 206, 225, 244, 255, 48, 49, 49, 68, 70, 89, + 108, 127, 146, 165, 184, 203, 206, 224, 243, 255, 47, 48, 48, 67, 70, 88, 107, 126, 145, 164, 183, + 202, 221, 223, 242, 255, 255, 47, 47, 66, 85, 87, 106, 125, 144, 163, 182, 201, 220, 223, 241, 255, + 255, 45, 47, 65, 84, 87, 105, 124, 143, 162, 181, 200, 219, 238, 240, 255, 255, 45, 45, 65, 83, + 102, 104, 123, 142, 161, 180, 199, 218, 237, 240, 255, 255, 44, 45, 45, 83, 101, 104, 122, 141, 160, + 179, 198, 217, 236, 255, 255, 255, 43, 44, 44, 63, 101, 119, 121, 140, 159, 178, 197, 216, 235, 254, + 255, 255, 42, 43, 43, 62, 81, 119, 121, 139, 158, 177, 196, 215, 234, 253, 255, 255, 41, 42, 42, + 61, 80, 99, 136, 138, 157, 176, 195, 214, 233, 252, 255, 255, 40, 41, 41, 60, 79, 98, 117, 138, + 156, 175, 194, 213, 232, 251, 255, 255, 40, 40, 40, 59, 78, 97, 116, 135}; + + vpImage I2_check_eroded1(image_data2_eroded1, 17, 17, true); if (I2_check_eroded1 != I_magic_square_erosion1_sse) { printMatrix(I_magic_square_erosion1_sse, "I_magic_square_erosion1_sse"); - throw vpException(vpException::fatalError, - "(I2_check_eroded1 != I_magic_square_erosion1_sse)"); + throw vpException(vpException::fatalError, "(I2_check_eroded1 != I_magic_square_erosion1_sse)"); } unsigned char image_data2_eroded2[17 * 17] = { - 192, 192, 211, 230, 249, 255, 255, 255, 57, 57, 57, 76, 95, 114, - 133, 152, 154, 192, 192, 211, 230, 249, 255, 255, 74, 57, 57, 57, - 76, 95, 114, 133, 152, 154, 210, 210, 229, 248, 255, 255, 73, 73, - 73, 74, 75, 94, 113, 132, 151, 170, 172, 228, 228, 247, 255, 255, - 72, 72, 72, 73, 74, 93, 112, 131, 150, 169, 171, 190, 246, 246, - 255, 255, 71, 71, 71, 72, 73, 92, 111, 130, 149, 168, 187, 189, - 208, 255, 255, 255, 70, 70, 70, 71, 72, 91, 110, 129, 148, 167, - 186, 188, 207, 226, 255, 255, 69, 69, 69, 70, 71, 90, 109, 128, - 147, 166, 185, 204, 206, 225, 244, 255, 85, 69, 69, 69, 70, 89, - 108, 127, 146, 165, 184, 203, 205, 224, 243, 255, 65, 65, 69, 69, - 69, 88, 107, 126, 145, 164, 183, 202, 221, 223, 242, 255, 255, 65, - 65, 84, 85, 87, 106, 125, 144, 163, 182, 201, 220, 222, 241, 255, - 255, 255, 65, 65, 84, 86, 105, 124, 143, 162, 181, 200, 219, 238, - 240, 255, 255, 63, 63, 83, 83, 102, 104, 123, 142, 161, 180, 199, - 218, 237, 239, 255, 255, 81, 63, 63, 101, 101, 103, 122, 141, 160, - 179, 198, 217, 236, 255, 255, 255, 80, 80, 63, 63, 119, 119, 121, - 140, 159, 178, 197, 216, 235, 254, 255, 255, 79, 79, 79, 80, 81, - 120, 120, 139, 158, 177, 196, 215, 234, 253, 255, 255, 78, 78, 78, - 79, 80, 99, 138, 138, 157, 176, 195, 214, 233, 252, 255, 255, 59, - 59, 59, 78, 79, 98, 117, 156, 156, 175, 194, 213, 232, 251, 255, - 255, 255, 59, 59, 59, 78, 97, 116, 135}; - - vpImage I2_check_eroded2(image_data2_eroded2, 17, 17, - true); + 192, 192, 211, 230, 249, 255, 255, 255, 57, 57, 57, 76, 95, 114, 133, 152, 154, 192, 192, 211, 230, + 249, 255, 255, 74, 57, 57, 57, 76, 95, 114, 133, 152, 154, 210, 210, 229, 248, 255, 255, 73, 73, + 73, 74, 75, 94, 113, 132, 151, 170, 172, 228, 228, 247, 255, 255, 72, 72, 72, 73, 74, 93, 112, + 131, 150, 169, 171, 190, 246, 246, 255, 255, 71, 71, 71, 72, 73, 92, 111, 130, 149, 168, 187, 189, + 208, 255, 255, 255, 70, 70, 70, 71, 72, 91, 110, 129, 148, 167, 186, 188, 207, 226, 255, 255, 69, + 69, 69, 70, 71, 90, 109, 128, 147, 166, 185, 204, 206, 225, 244, 255, 85, 69, 69, 69, 70, 89, + 108, 127, 146, 165, 184, 203, 205, 224, 243, 255, 65, 65, 69, 69, 69, 88, 107, 126, 145, 164, 183, + 202, 221, 223, 242, 255, 255, 65, 65, 84, 85, 87, 106, 125, 144, 163, 182, 201, 220, 222, 241, 255, + 255, 255, 65, 65, 84, 86, 105, 124, 143, 162, 181, 200, 219, 238, 240, 255, 255, 63, 63, 83, 83, + 102, 104, 123, 142, 161, 180, 199, 218, 237, 239, 255, 255, 81, 63, 63, 101, 101, 103, 122, 141, 160, + 179, 198, 217, 236, 255, 255, 255, 80, 80, 63, 63, 119, 119, 121, 140, 159, 178, 197, 216, 235, 254, + 255, 255, 79, 79, 79, 80, 81, 120, 120, 139, 158, 177, 196, 215, 234, 253, 255, 255, 78, 78, 78, + 79, 80, 99, 138, 138, 157, 176, 195, 214, 233, 252, 255, 255, 59, 59, 59, 78, 79, 98, 117, 156, + 156, 175, 194, 213, 232, 251, 255, 255, 255, 59, 59, 59, 78, 97, 116, 135}; + + vpImage I2_check_eroded2(image_data2_eroded2, 17, 17, true); if (I2_check_eroded2 != I_magic_square_erosion2_sse) { - throw vpException(vpException::fatalError, - "(I2_check_eroded2 != I_magic_square_erosion2_sse)"); + throw vpException(vpException::fatalError, "(I2_check_eroded2 != I_magic_square_erosion2_sse)"); } std::cout << std::endl; @@ -724,62 +626,53 @@ int main(int argc, const char **argv) // implementation) vpImage I_Klimt_binarized = I_Klimt; - vpImageTools::binarise(I_Klimt_binarized, (unsigned char)127, - (unsigned char)127, (unsigned char)0, + vpImageTools::binarise(I_Klimt_binarized, (unsigned char)127, (unsigned char)127, (unsigned char)0, (unsigned char)1, (unsigned char)1, true); // Dilatation CONNEXITY_4 vpImage I_Klimt_binarized_dilatation1 = I_Klimt_binarized; - vpImage I_Klimt_binarized_dilatation1_sse = - I_Klimt_binarized; + vpImage I_Klimt_binarized_dilatation1_sse = I_Klimt_binarized; double t = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - vpImageMorphology::dilatation(I_Klimt_binarized_dilatation1, - (unsigned char)1, (unsigned char)0, + vpImageMorphology::dilatation(I_Klimt_binarized_dilatation1, (unsigned char)1, (unsigned char)0, vpImageMorphology::CONNEXITY_4); } t = vpTime::measureTimeMs() - t; double t_sse = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - vpImageMorphology::dilatation(I_Klimt_binarized_dilatation1_sse, - vpImageMorphology::CONNEXITY_4); + vpImageMorphology::dilatation(I_Klimt_binarized_dilatation1_sse, vpImageMorphology::CONNEXITY_4); } t_sse = vpTime::measureTimeMs() - t_sse; std::cout << "(I_Klimt_binarized_dilatation1 == " "I_Klimt_binarized_dilatation1_sse)? " - << (I_Klimt_binarized_dilatation1 == - I_Klimt_binarized_dilatation1_sse) - << " ; t=" << t << " ms ; t_sse=" << t_sse << " ms" + << (I_Klimt_binarized_dilatation1 == I_Klimt_binarized_dilatation1_sse) << " ; t=" << t + << " ms ; t_sse=" << t_sse << " ms" << " ; speed-up=" << (t / t_sse) << "X" << std::endl; // Dilatation CONNEXITY_8 vpImage I_Klimt_binarized_dilatation2 = I_Klimt_binarized; - vpImage I_Klimt_binarized_dilatation2_sse = - I_Klimt_binarized; + vpImage I_Klimt_binarized_dilatation2_sse = I_Klimt_binarized; t = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - vpImageMorphology::dilatation(I_Klimt_binarized_dilatation2, - (unsigned char)1, (unsigned char)0, + vpImageMorphology::dilatation(I_Klimt_binarized_dilatation2, (unsigned char)1, (unsigned char)0, vpImageMorphology::CONNEXITY_8); } t = vpTime::measureTimeMs() - t; t_sse = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - vpImageMorphology::dilatation(I_Klimt_binarized_dilatation2_sse, - vpImageMorphology::CONNEXITY_8); + vpImageMorphology::dilatation(I_Klimt_binarized_dilatation2_sse, vpImageMorphology::CONNEXITY_8); } t_sse = vpTime::measureTimeMs() - t_sse; std::cout << "(I_Klimt_binarized_dilatation2 == " "I_Klimt_binarized_dilatation2_sse)? " - << (I_Klimt_binarized_dilatation2 == - I_Klimt_binarized_dilatation2_sse) - << " ; t=" << t << " ms ; t_sse=" << t_sse << " ms" + << (I_Klimt_binarized_dilatation2 == I_Klimt_binarized_dilatation2_sse) << " ; t=" << t + << " ms ; t_sse=" << t_sse << " ms" << " ; speed-up=" << (t / t_sse) << "X" << std::endl; // Erosion CONNEXITY_4 @@ -788,24 +681,21 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - vpImageMorphology::erosion(I_Klimt_binarized_erosion1, (unsigned char)1, - (unsigned char)0, + vpImageMorphology::erosion(I_Klimt_binarized_erosion1, (unsigned char)1, (unsigned char)0, vpImageMorphology::CONNEXITY_4); } t = vpTime::measureTimeMs() - t; t_sse = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - vpImageMorphology::erosion(I_Klimt_binarized_erosion1_sse, - vpImageMorphology::CONNEXITY_4); + vpImageMorphology::erosion(I_Klimt_binarized_erosion1_sse, vpImageMorphology::CONNEXITY_4); } t_sse = vpTime::measureTimeMs() - t_sse; - std::cout - << "(I_Klimt_binarized_erosion1 == I_Klimt_binarized_erosion1_sse)? " - << (I_Klimt_binarized_erosion1 == I_Klimt_binarized_erosion1_sse) - << " ; t=" << t << " ms ; t_sse=" << t_sse << " ms" - << " ; speed-up=" << (t / t_sse) << "X" << std::endl; + std::cout << "(I_Klimt_binarized_erosion1 == I_Klimt_binarized_erosion1_sse)? " + << (I_Klimt_binarized_erosion1 == I_Klimt_binarized_erosion1_sse) << " ; t=" << t + << " ms ; t_sse=" << t_sse << " ms" + << " ; speed-up=" << (t / t_sse) << "X" << std::endl; // Erosion CONNEXITY_8 vpImage I_Klimt_binarized_erosion2 = I_Klimt_binarized; @@ -813,24 +703,21 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - vpImageMorphology::erosion(I_Klimt_binarized_erosion2, (unsigned char)1, - (unsigned char)0, + vpImageMorphology::erosion(I_Klimt_binarized_erosion2, (unsigned char)1, (unsigned char)0, vpImageMorphology::CONNEXITY_8); } t = vpTime::measureTimeMs() - t; t_sse = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - vpImageMorphology::erosion(I_Klimt_binarized_erosion2_sse, - vpImageMorphology::CONNEXITY_8); + vpImageMorphology::erosion(I_Klimt_binarized_erosion2_sse, vpImageMorphology::CONNEXITY_8); } t_sse = vpTime::measureTimeMs() - t_sse; - std::cout - << "(I_Klimt_binarized_erosion2 == I_Klimt_binarized_erosion2_sse)? " - << (I_Klimt_binarized_erosion2 == I_Klimt_binarized_erosion2_sse) - << " ; t=" << t << " ms ; t_sse=" << t_sse << " ms" - << " ; speed-up=" << (t / t_sse) << "X" << std::endl; + std::cout << "(I_Klimt_binarized_erosion2 == I_Klimt_binarized_erosion2_sse)? " + << (I_Klimt_binarized_erosion2 == I_Klimt_binarized_erosion2_sse) << " ; t=" << t + << " ms ; t_sse=" << t_sse << " ms" + << " ; speed-up=" << (t / t_sse) << "X" << std::endl; // Benchmark on grayscale images (compare regular code with SSE // implementation) @@ -847,14 +734,12 @@ int main(int argc, const char **argv) t_sse = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - vpImageMorphology::dilatation(I_Klimt_dilatation1_sse, - vpImageMorphology::CONNEXITY_4); + vpImageMorphology::dilatation(I_Klimt_dilatation1_sse, vpImageMorphology::CONNEXITY_4); } t_sse = vpTime::measureTimeMs() - t_sse; std::cout << "(I_Klimt_dilatation1 == I_Klimt_dilatation1_sse)? " - << (I_Klimt_dilatation1 == I_Klimt_dilatation1_sse) - << " ; t=" << t << " ms ; t_sse=" << t_sse << " ms" + << (I_Klimt_dilatation1 == I_Klimt_dilatation1_sse) << " ; t=" << t << " ms ; t_sse=" << t_sse << " ms" << " ; speed-up=" << (t / t_sse) << "X" << std::endl; // Dilatation CONNEXITY_8 grayscale @@ -869,14 +754,12 @@ int main(int argc, const char **argv) t_sse = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - vpImageMorphology::dilatation(I_Klimt_dilatation2_sse, - vpImageMorphology::CONNEXITY_8); + vpImageMorphology::dilatation(I_Klimt_dilatation2_sse, vpImageMorphology::CONNEXITY_8); } t_sse = vpTime::measureTimeMs() - t_sse; std::cout << "(I_Klimt_dilatation2 == I_Klimt_dilatation2_sse)? " - << (I_Klimt_dilatation2 == I_Klimt_dilatation2_sse) - << " ; t=" << t << " ms ; t_sse=" << t_sse << " ms" + << (I_Klimt_dilatation2 == I_Klimt_dilatation2_sse) << " ; t=" << t << " ms ; t_sse=" << t_sse << " ms" << " ; speed-up=" << (t / t_sse) << "X" << std::endl; // Erosion CONNEXITY_4 grayscale @@ -891,14 +774,12 @@ int main(int argc, const char **argv) t_sse = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - vpImageMorphology::erosion(I_Klimt_erosion1_sse, - vpImageMorphology::CONNEXITY_4); + vpImageMorphology::erosion(I_Klimt_erosion1_sse, vpImageMorphology::CONNEXITY_4); } t_sse = vpTime::measureTimeMs() - t_sse; - std::cout << "(I_Klimt_erosion1 == I_Klimt_erosion1_sse)? " - << (I_Klimt_erosion1 == I_Klimt_erosion1_sse) << " ; t=" << t - << " ms ; t_sse=" << t_sse << " ms" + std::cout << "(I_Klimt_erosion1 == I_Klimt_erosion1_sse)? " << (I_Klimt_erosion1 == I_Klimt_erosion1_sse) + << " ; t=" << t << " ms ; t_sse=" << t_sse << " ms" << " ; speed-up=" << (t / t_sse) << "X" << std::endl; // Erosion CONNEXITY_8 grayscale @@ -913,24 +794,20 @@ int main(int argc, const char **argv) t_sse = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - vpImageMorphology::erosion(I_Klimt_erosion2_sse, - vpImageMorphology::CONNEXITY_8); + vpImageMorphology::erosion(I_Klimt_erosion2_sse, vpImageMorphology::CONNEXITY_8); } t_sse = vpTime::measureTimeMs() - t_sse; - std::cout << "(I_Klimt_erosion2 == I_Klimt_erosion2_sse)? " - << (I_Klimt_erosion2 == I_Klimt_erosion2_sse) << " ; t=" << t - << " ms ; t_sse=" << t_sse << " ms" + std::cout << "(I_Klimt_erosion2 == I_Klimt_erosion2_sse)? " << (I_Klimt_erosion2 == I_Klimt_erosion2_sse) + << " ; t=" << t << " ms ; t_sse=" << t_sse << " ms" << " ; speed-up=" << (t / t_sse) << "X" << std::endl; // Compare with OpenCV #if (VISP_HAVE_OPENCV_VERSION >= 0x030000) std::cout << std::endl; - cv::Mat cross_SE = - cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3, 3)); - cv::Mat rect_SE = - cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); + cv::Mat cross_SE = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3, 3)); + cv::Mat rect_SE = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); // Dilatation CONNEXITY_4 grayscale cv::Mat matImg_dilatation1; @@ -938,16 +815,14 @@ int main(int argc, const char **argv) double t_opencv = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - cv::morphologyEx(matImg_dilatation1, matImg_dilatation1, - cv::MORPH_DILATE, cross_SE); + cv::morphologyEx(matImg_dilatation1, matImg_dilatation1, cv::MORPH_DILATE, cross_SE); } t_opencv = vpTime::measureTimeMs() - t_opencv; vpImage I_matImg_dilatation1; vpImageConvert::convert(matImg_dilatation1, I_matImg_dilatation1); std::cout << "(I_matImg_dilatation1 == I_Klimt_dilatation1_sse)? " - << (I_matImg_dilatation1 == I_Klimt_dilatation1_sse) - << " ; t_opencv=" << t_opencv << " ms" << std::endl; + << (I_matImg_dilatation1 == I_Klimt_dilatation1_sse) << " ; t_opencv=" << t_opencv << " ms" << std::endl; // Dilatation CONNEXITY_8 grayscale cv::Mat matImg_dilatation2; @@ -955,16 +830,14 @@ int main(int argc, const char **argv) t_opencv = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - cv::morphologyEx(matImg_dilatation2, matImg_dilatation2, - cv::MORPH_DILATE, rect_SE); + cv::morphologyEx(matImg_dilatation2, matImg_dilatation2, cv::MORPH_DILATE, rect_SE); } t_opencv = vpTime::measureTimeMs() - t_opencv; vpImage I_matImg_dilatation2; vpImageConvert::convert(matImg_dilatation2, I_matImg_dilatation2); std::cout << "(I_matImg_dilatation2 == I_Klimt_dilatation2_sse)? " - << (I_matImg_dilatation2 == I_Klimt_dilatation2_sse) - << " ; t_opencv=" << t_opencv << " ms" << std::endl; + << (I_matImg_dilatation2 == I_Klimt_dilatation2_sse) << " ; t_opencv=" << t_opencv << " ms" << std::endl; // Erosion CONNEXITY_4 grayscale cv::Mat matImg_erosion1; @@ -972,15 +845,13 @@ int main(int argc, const char **argv) t_opencv = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - cv::morphologyEx(matImg_erosion1, matImg_erosion1, cv::MORPH_ERODE, - cross_SE); + cv::morphologyEx(matImg_erosion1, matImg_erosion1, cv::MORPH_ERODE, cross_SE); } t_opencv = vpTime::measureTimeMs() - t_opencv; vpImage I_matImg_erosion1; vpImageConvert::convert(matImg_erosion1, I_matImg_erosion1); - std::cout << "(I_matImg_erosion1 == I_Klimt_erosion1_sse)? " - << (I_matImg_erosion1 == I_Klimt_erosion1_sse) + std::cout << "(I_matImg_erosion1 == I_Klimt_erosion1_sse)? " << (I_matImg_erosion1 == I_Klimt_erosion1_sse) << " ; t_opencv=" << t_opencv << " ms" << std::endl; // Erosion CONNEXITY_8 grayscale @@ -989,15 +860,13 @@ int main(int argc, const char **argv) t_opencv = vpTime::measureTimeMs(); for (int cpt = 0; cpt < nbIterations; cpt++) { - cv::morphologyEx(matImg_erosion2, matImg_erosion2, cv::MORPH_ERODE, - rect_SE); + cv::morphologyEx(matImg_erosion2, matImg_erosion2, cv::MORPH_ERODE, rect_SE); } t_opencv = vpTime::measureTimeMs() - t_opencv; vpImage I_matImg_erosion2; vpImageConvert::convert(matImg_erosion2, I_matImg_erosion2); - std::cout << "(I_matImg_erosion2 == I_Klimt_erosion2_sse)? " - << (I_matImg_erosion2 == I_Klimt_erosion2_sse) + std::cout << "(I_matImg_erosion2 == I_Klimt_erosion2_sse)? " << (I_matImg_erosion2 == I_Klimt_erosion2_sse) << " ; t_opencv=" << t_opencv << " ms" << std::endl; #endif @@ -1007,7 +876,6 @@ int main(int argc, const char **argv) return EXIT_FAILURE; } - std::cout << "\nTest of morphology erosion / dilatation functions are OK!" - << std::endl; + std::cout << "\nTest of morphology erosion / dilatation functions are OK!" << std::endl; return EXIT_SUCCESS; } diff --git a/modules/core/test/image/testImageResize.cpp b/modules/core/test/image/testImageResize.cpp index 05bf1e2aca..181af54e70 100644 --- a/modules/core/test/image/testImageResize.cpp +++ b/modules/core/test/image/testImageResize.cpp @@ -66,8 +66,7 @@ namespace \param h : Resize height. \param m : Resize interpolation method. */ -void usage(const char *name, const char *badparam, std::string ipath, - unsigned int &w, unsigned int &h, int &m) +void usage(const char *name, const char *badparam, std::string ipath, unsigned int &w, unsigned int &h, int &m) { fprintf(stdout, "\n\ Test image resize.\n\ @@ -122,8 +121,7 @@ void usage(const char *name, const char *badparam, std::string ipath, \param opt_click : Do not need click if set. \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - unsigned int &w, unsigned int &h, int &method, +bool getOptions(int argc, const char **argv, std::string &ipath, unsigned int &w, unsigned int &h, int &method, bool &opt_display, bool &opt_click) { const char *optarg_; @@ -196,8 +194,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, width, height, method, opt_display, - opt_click) == false) { + if (getOptions(argc, argv, opt_ipath, width, height, method, opt_display, opt_click) == false) { exit(EXIT_FAILURE); } @@ -211,8 +208,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -221,13 +217,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath, width, height, method); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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(EXIT_FAILURE); } @@ -241,35 +234,27 @@ int main(int argc, const char **argv) for (unsigned int cpt = 0; cpt < Itest.getSize(); cpt++) { Itest.bitmap[cpt] = cpt; } - vpImage Itest_resize(Itest.getHeight() * 2, - Itest.getWidth() * 2), + vpImage Itest_resize(Itest.getHeight() * 2, Itest.getWidth() * 2), Itest_resize2(Itest.getHeight(), Itest.getWidth()); - vpImageTools::resize(Itest, Itest_resize, - (vpImageTools::vpImageInterpolationType)m); - vpImageTools::resize(Itest_resize, Itest_resize2, - (vpImageTools::vpImageInterpolationType)m); + vpImageTools::resize(Itest, Itest_resize, (vpImageTools::vpImageInterpolationType)m); + vpImageTools::resize(Itest_resize, Itest_resize2, (vpImageTools::vpImageInterpolationType)m); std::cout << "Itest:\n" << Itest << std::endl; std::cout << "Itest_resize:\n" << Itest_resize << std::endl; std::cout << "Itest_resize2:\n" << Itest_resize2 << std::endl; - std::cout << "(Itest ==Itest_resize2)? " << (Itest == Itest_resize2) - << std::endl; + std::cout << "(Itest ==Itest_resize2)? " << (Itest == Itest_resize2) << std::endl; Itest.resize(4, 4); for (unsigned int cpt = 0; cpt < Itest.getSize(); cpt++) { Itest.bitmap[cpt] = cpt; } - vpImageTools::resize(Itest, Itest_resize, Itest.getWidth() / 2, - Itest.getHeight() / 2, + vpImageTools::resize(Itest, Itest_resize, Itest.getWidth() / 2, Itest.getHeight() / 2, (vpImageTools::vpImageInterpolationType)m); - vpImageTools::resize(Itest_resize, Itest_resize2, Itest.getWidth(), - Itest.getHeight(), + vpImageTools::resize(Itest_resize, Itest_resize2, Itest.getWidth(), Itest.getHeight(), (vpImageTools::vpImageInterpolationType)m); std::cout << "\nItest:\n" << Itest << std::endl; std::cout << "Itest_resize:\n" << Itest_resize << std::endl; std::cout << "Itest_resize2:\n" << Itest_resize2 << std::endl; - std::cout << "(Itest ==Itest_resize2)? " << (Itest == Itest_resize2) - << std::endl - << std::endl; + std::cout << "(Itest ==Itest_resize2)? " << (Itest == Itest_resize2) << std::endl << std::endl; } // Grayscale image @@ -282,12 +267,10 @@ int main(int argc, const char **argv) vpImage I_resize; double t = vpTime::measureTimeMs(); - vpImageTools::resize(I, I_resize, width, height, - (vpImageTools::vpImageInterpolationType)method); + vpImageTools::resize(I, I_resize, width, height, (vpImageTools::vpImageInterpolationType)method); t = vpTime::measureTimeMs() - t; - std::cout << "Time to resize from " << I.getWidth() << "x" - << I.getHeight() << " to " << width << "x" << height << ": " - << t << " ms" << std::endl; + std::cout << "Time to resize from " << I.getWidth() << "x" << I.getHeight() << " to " << width << "x" << height + << ": " << t << " ms" << std::endl; #if defined(VISP_HAVE_X11) vpDisplayX *d1 = new vpDisplayX, *d2 = new vpDisplayX; @@ -305,18 +288,15 @@ int main(int argc, const char **argv) #endif if (opt_display) { -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || \ - defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || \ +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || \ defined(VISP_HAVE_D3D9) d1->init(I, 0, 0, "Grayscale image"); - d2->init(I_resize, (int)I.getWidth() + 80, 0, - "Grayscale image resized"); + d2->init(I_resize, (int)I.getWidth() + 80, 0, "Grayscale image resized"); #endif vpDisplay::display(I); vpDisplay::display(I_resize); - vpDisplay::displayText(I_resize, 20, 20, "Click to continue.", - vpColor::red); + vpDisplay::displayText(I_resize, 20, 20, "Click to continue.", vpColor::red); vpDisplay::flush(I); vpDisplay::flush(I_resize); @@ -335,12 +315,10 @@ int main(int argc, const char **argv) vpImage I_color_resize; t = vpTime::measureTimeMs(); - vpImageTools::resize(I_color, I_color_resize, width, height, - (vpImageTools::vpImageInterpolationType)method); + vpImageTools::resize(I_color, I_color_resize, width, height, (vpImageTools::vpImageInterpolationType)method); t = vpTime::measureTimeMs() - t; - std::cout << "Time to resize from " << I_color.getWidth() << "x" - << I_color.getHeight() << " to " << width << "x" << height - << ": " << t << " ms" << std::endl; + std::cout << "Time to resize from " << I_color.getWidth() << "x" << I_color.getHeight() << " to " << width << "x" + << height << ": " << t << " ms" << std::endl; #if defined(VISP_HAVE_X11) vpDisplayX *d3 = new vpDisplayX, *d4 = new vpDisplayX; @@ -358,18 +336,15 @@ int main(int argc, const char **argv) #endif if (opt_display) { -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || \ - defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || \ +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || \ defined(VISP_HAVE_D3D9) d3->init(I_color, 0, 0, "Color image"); - d4->init(I_color_resize, (int)I_color.getWidth() + 80, 0, - "Color image resized"); + d4->init(I_color_resize, (int)I_color.getWidth() + 80, 0, "Color image resized"); #endif vpDisplay::display(I_color); vpDisplay::display(I_color_resize); - vpDisplay::displayText(I_color_resize, 20, 20, "Click to quit.", - vpColor::red); + vpDisplay::displayText(I_color_resize, 20, 20, "Click to quit.", vpColor::red); vpDisplay::flush(I_color); vpDisplay::flush(I_color_resize); if (opt_click) { @@ -377,8 +352,7 @@ int main(int argc, const char **argv) } } -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || \ - defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || \ +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || \ defined(VISP_HAVE_D3D9) delete d1; delete d2; @@ -387,14 +361,11 @@ int main(int argc, const char **argv) #endif vpImage I_color_double, I_color_double_half; - vpImageTools::resize(I_color, I_color_double, I_color.getWidth() * 2, - I_color.getHeight() * 2, + vpImageTools::resize(I_color, I_color_double, I_color.getWidth() * 2, I_color.getHeight() * 2, (vpImageTools::vpImageInterpolationType)method); - vpImageTools::resize(I_color_double, I_color_double_half, - I_color.getWidth(), I_color.getHeight(), + vpImageTools::resize(I_color_double, I_color_double_half, I_color.getWidth(), I_color.getHeight(), (vpImageTools::vpImageInterpolationType)method); - std::cout << "\n(I_color == I_color_double_half)? " - << (I_color == I_color_double_half) << std::endl; + std::cout << "\n(I_color == I_color_double_half)? " << (I_color == I_color_double_half) << std::endl; double root_mean_square_error = 0.0; for (unsigned int i = 0; i < I_color.getHeight(); i++) { @@ -403,19 +374,14 @@ int main(int argc, const char **argv) root_mean_square_error += c_error.sumSquare(); } } - std::cout << "Root Mean Square Error: " - << sqrt(root_mean_square_error / (I_color.getSize() * 3)) - << std::endl; + std::cout << "Root Mean Square Error: " << sqrt(root_mean_square_error / (I_color.getSize() * 3)) << std::endl; vpImage I_color_half, I_color_half_double; - vpImageTools::resize(I_color, I_color_half, I_color.getWidth() / 2, - I_color.getHeight() / 2, + vpImageTools::resize(I_color, I_color_half, I_color.getWidth() / 2, I_color.getHeight() / 2, (vpImageTools::vpImageInterpolationType)method); - vpImageTools::resize(I_color_half, I_color_half_double, - I_color.getWidth(), I_color.getHeight(), + vpImageTools::resize(I_color_half, I_color_half_double, I_color.getWidth(), I_color.getHeight(), (vpImageTools::vpImageInterpolationType)method); - std::cout << "\n(I_color == I_color_half_double)? " - << (I_color == I_color_half_double) << std::endl; + std::cout << "\n(I_color == I_color_half_double)? " << (I_color == I_color_half_double) << std::endl; root_mean_square_error = 0.0; for (unsigned int i = 0; i < I_color.getHeight(); i++) { @@ -424,9 +390,7 @@ int main(int argc, const char **argv) root_mean_square_error += c_error.sumSquare(); } } - std::cout << "Root Mean Square Error: " - << sqrt(root_mean_square_error / (I_color.getSize() * 3)) - << std::endl; + std::cout << "Root Mean Square Error: " << sqrt(root_mean_square_error / (I_color.getSize() * 3)) << std::endl; return EXIT_SUCCESS; } catch (const vpException &e) { diff --git a/modules/core/test/image/testIoPGM.cpp b/modules/core/test/image/testIoPGM.cpp index fc46df313b..05e53fd9c2 100644 --- a/modules/core/test/image/testIoPGM.cpp +++ b/modules/core/test/image/testIoPGM.cpp @@ -66,8 +66,8 @@ \param user : Username. */ -void usage(const char *name, const char *badparam, const std::string &ipath, - const std::string &opath, const std::string &user) +void usage(const char *name, const char *badparam, const std::string &ipath, const std::string &opath, + const std::string &user) { fprintf(stdout, "\n\ Read and write PGM images on the disk. Also test exceptions.\n\ @@ -112,8 +112,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, const std::string &user) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, const std::string &user) { const char *optarg_; int c; @@ -205,8 +204,7 @@ int main(int argc, const char **argv) 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; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; exit(-1); } } @@ -217,8 +215,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -227,13 +224,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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); } @@ -256,24 +250,20 @@ int main(int argc, const char **argv) try { // Try to load a non existing image (test for exceptions) // Load a non existing grey image - filename = - vpIoTools::createFilePath(ipath, "image-that-does-not-exist.pgm"); + filename = vpIoTools::createFilePath(ipath, "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; + 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"); + 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; + std::cout << "Catch an exception due to a non existing file: " << e << std::endl; } return 0; } catch (vpException &e) { diff --git a/modules/core/test/image/testIoPPM.cpp b/modules/core/test/image/testIoPPM.cpp index 1da267b82c..5010d37015 100644 --- a/modules/core/test/image/testIoPPM.cpp +++ b/modules/core/test/image/testIoPPM.cpp @@ -67,8 +67,8 @@ \param user : Username. */ -void usage(const char *name, const char *badparam, const std::string &ipath, - const std::string &opath, const std::string &user) +void usage(const char *name, const char *badparam, const std::string &ipath, const std::string &opath, + const std::string &user) { fprintf(stdout, "\n\ Read and write PPM images on the disk. Also test exceptions.\n\ @@ -114,8 +114,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, const std::string &user) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, const std::string &user) { const char *optarg_; int c; @@ -207,8 +206,7 @@ int main(int argc, const char **argv) 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; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; exit(-1); } } @@ -219,8 +217,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -229,13 +226,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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); } @@ -259,8 +253,7 @@ int main(int argc, const char **argv) // Try to load a non existing image (test for exceptions) try { // Load a non existing grey image - filename = - vpIoTools::createFilePath(ipath, "image-that-does-not-exist.ppm"); + filename = vpIoTools::createFilePath(ipath, "image-that-does-not-exist.ppm"); std::cout << "Read image: " << filename << std::endl; vpImageIo::read(I, filename); } catch (vpImageException &e) { @@ -270,13 +263,11 @@ int main(int argc, const char **argv) // Try to write an image to a non existing directory try { - filename = vpIoTools::createFilePath( - opath, "directory-that-does-not-exist/Klimt.ppm"); + 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; + std::cout << "Catch an exception due to a non existing file: " << e << std::endl; } ///////////////////////////////////////////////////////////////////// @@ -295,24 +286,20 @@ int main(int argc, const char **argv) try { // Try to load a non existing image (test for exceptions) // Load a non existing color image - filename = - vpIoTools::createFilePath(ipath, "image-that-does-not-exist.ppm"); + filename = vpIoTools::createFilePath(ipath, "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; + 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"); + 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; + std::cout << "Catch an exception due to a non existing file: " << e << std::endl; } return 0; } catch (vpException &e) { diff --git a/modules/core/test/image/testPerformanceLUT.cpp b/modules/core/test/image/testPerformanceLUT.cpp index b79967ac3a..722caf73ac 100644 --- a/modules/core/test/image/testPerformanceLUT.cpp +++ b/modules/core/test/image/testPerformanceLUT.cpp @@ -64,8 +64,8 @@ \param user : Username. */ -void usage(const char *name, const char *badparam, const std::string &ipath, - const std::string &opath, const std::string &user) +void usage(const char *name, const char *badparam, const std::string &ipath, const std::string &opath, + const std::string &user) { fprintf(stdout, "\n\ Test performance between methods to iterate over pixel image.\n\ @@ -114,8 +114,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, const std::string &user, +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, const std::string &user, unsigned int &nbThreads) { const char *optarg_; @@ -167,8 +166,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, \param alpha : Gain. \param beta: Offset. */ -void iterate_method1(vpImage &I, const double alpha, - const double beta) +void iterate_method1(vpImage &I, const double alpha, const double beta) { unsigned int size = I.getWidth() * I.getHeight(); unsigned char *ptrStart = (unsigned char *)I.bitmap; @@ -176,8 +174,7 @@ void iterate_method1(vpImage &I, const double alpha, unsigned char *ptrCurrent = ptrStart; while (ptrCurrent != ptrEnd) { - *ptrCurrent = - vpMath::saturate((*ptrCurrent) * alpha + beta); + *ptrCurrent = vpMath::saturate((*ptrCurrent) * alpha + beta); ++ptrCurrent; } } @@ -190,8 +187,7 @@ void iterate_method1(vpImage &I, const double alpha, \param alpha : Gain. \param beta: Offset. */ -void iterate_method1(vpImage &I, const double alpha, - const double beta) +void iterate_method1(vpImage &I, const double alpha, const double beta) { unsigned int size = I.getWidth() * I.getHeight(); unsigned char *ptrStart = (unsigned char *)I.bitmap; @@ -199,8 +195,7 @@ void iterate_method1(vpImage &I, const double alpha, unsigned char *ptrCurrent = ptrStart; while (ptrCurrent != ptrEnd) { - *ptrCurrent = - vpMath::saturate((*ptrCurrent) * alpha + beta); + *ptrCurrent = vpMath::saturate((*ptrCurrent) * alpha + beta); ++ptrCurrent; } } @@ -213,8 +208,7 @@ void iterate_method1(vpImage &I, const double alpha, \param alpha : Gain. \param beta: Offset. */ -void iterate_method2(vpImage &I, const double alpha, - const double beta) +void iterate_method2(vpImage &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++) { @@ -257,8 +251,7 @@ int main(int argc, const char **argv) vpIoTools::getUserName(username); // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_opath, username, nbThreads) == - false) { + if (getOptions(argc, argv, opt_ipath, opt_opath, username, nbThreads) == false) { exit(-1); } @@ -280,8 +273,7 @@ int main(int argc, const char **argv) 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; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; exit(-1); } } @@ -292,8 +284,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -302,13 +293,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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); } @@ -326,8 +314,7 @@ int main(int argc, const char **argv) vpImageIo::read(I_iterate2, filename); vpImageIo::read(I_lut, filename); - std::cout << "I=" << I_iterate1.getWidth() << "x" - << I_iterate1.getHeight() << std::endl; + std::cout << "I=" << I_iterate1.getWidth() << "x" << I_iterate1.getHeight() << std::endl; double alpha = 1.5, beta = -30.0; unsigned int nbIterations = 10; @@ -338,12 +325,10 @@ int main(int argc, const char **argv) 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; + std::cout << "t_iterate1=" << t_iterate1 << " ms ; t_iterate1/" << nbIterations << "=" + << (t_iterate1 / nbIterations) << " ms" << std::endl; - filename = - vpIoTools::createFilePath(opath, "Klimt_performance_iterate1.ppm"); + filename = vpIoTools::createFilePath(opath, "Klimt_performance_iterate1.ppm"); vpImageIo::write(I_iterate1, filename); // Iterate method 2 @@ -352,12 +337,10 @@ int main(int argc, const char **argv) 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; + std::cout << "t_iterate2=" << t_iterate2 << " ms ; t_iterate2/" << nbIterations << "=" + << (t_iterate2 / nbIterations) << " ms" << std::endl; - filename = - vpIoTools::createFilePath(opath, "Klimt_performance_iterate2.ppm"); + filename = vpIoTools::createFilePath(opath, "Klimt_performance_iterate2.ppm"); vpImageIo::write(I_iterate2, filename); // LUT method @@ -375,8 +358,8 @@ int main(int argc, const char **argv) I_lut.performLut(lut, nbThreads); } t_lut = vpTime::measureTimeMs() - t_lut; - std::cout << "t_lut=" << t_lut << " ms ; t_lut/" << nbIterations << "=" - << (t_lut / nbIterations) << " ms" << std::endl; + 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); @@ -385,8 +368,7 @@ int main(int argc, const char **argv) bool same = true; for (unsigned int i = 0; i < I_iterate1.getHeight() && same; i++) { for (unsigned int j = 0; j < I_iterate1.getWidth() && same; j++) { - if (I_iterate1[i][j] != I_iterate2[i][j] || - I_iterate1[i][j] != I_lut[i][j]) { + if (I_iterate1[i][j] != I_iterate2[i][j] || I_iterate1[i][j] != I_lut[i][j]) { same = false; } } @@ -406,8 +388,7 @@ int main(int argc, const char **argv) vpImageIo::read(I_iterate_grayscale1, filename); vpImageIo::read(I_lut_grayscale, filename); - std::cout << "I_grayscale=" << I_lut_grayscale.getWidth() << "x" - << I_lut_grayscale.getHeight() << std::endl; + std::cout << "I_grayscale=" << I_lut_grayscale.getWidth() << "x" << I_lut_grayscale.getHeight() << std::endl; // Iterate method 1 on grayscale double t_iterate_grayscale1 = vpTime::measureTimeMs(); @@ -415,12 +396,10 @@ int main(int argc, const char **argv) iterate_method1(I_iterate_grayscale1, alpha, beta); } t_iterate_grayscale1 = vpTime::measureTimeMs() - t_iterate_grayscale1; - std::cout << "t_iterate_grayscale1=" << t_iterate_grayscale1 - << " ms ; t_iterate1/" << nbIterations << "=" + std::cout << "t_iterate_grayscale1=" << t_iterate_grayscale1 << " ms ; t_iterate1/" << nbIterations << "=" << (t_iterate_grayscale1 / nbIterations) << " ms" << std::endl; - filename = vpIoTools::createFilePath( - opath, "Klimt_performance_iterate1_grayscale.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_performance_iterate1_grayscale.pgm"); vpImageIo::write(I_iterate_grayscale1, filename); // LUT method on grayscale @@ -435,12 +414,10 @@ int main(int argc, const char **argv) I_lut_grayscale.performLut(lut, nbThreads); } t_lut_grayscale = vpTime::measureTimeMs() - t_lut_grayscale; - std::cout << "t_lut_grayscale=" << t_lut_grayscale - << " ms ; t_lut_grayscale/" << nbIterations << "=" + std::cout << "t_lut_grayscale=" << t_lut_grayscale << " ms ; t_lut_grayscale/" << nbIterations << "=" << (t_lut_grayscale / nbIterations) << " ms" << std::endl; - filename = vpIoTools::createFilePath( - opath, "Klimt_performance_lut_grayscale.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_performance_lut_grayscale.pgm"); vpImageIo::write(I_lut_grayscale, filename); // Check grayscale image @@ -493,8 +470,8 @@ int main(int argc, const char **argv) } t_lut_singlethread = vpTime::measureTimeMs() - t_lut_singlethread; - std::cout << "\nt_lut_singlethread/t_lut_multithread (color)=" - << t_lut_singlethread / t_lut_multithread << "X" << std::endl; + std::cout << "\nt_lut_singlethread/t_lut_multithread (color)=" << t_lut_singlethread / t_lut_multithread << "X" + << std::endl; // Computation time on grayscale image vpImageIo::read(I_lut_grayscale, filename); @@ -525,8 +502,8 @@ int main(int argc, const char **argv) } t_lut_singlethread = vpTime::measureTimeMs() - t_lut_singlethread; - std::cout << "\nt_lut_singlethread/t_lut_multithread (grayscale)=" - << t_lut_singlethread / t_lut_multithread << "X" << std::endl; + std::cout << "\nt_lut_singlethread/t_lut_multithread (grayscale)=" << t_lut_singlethread / t_lut_multithread << "X" + << std::endl; // Check performLut with multithreading and image size not divisible by 8 vpImage I_test_grayscale(49, 7); diff --git a/modules/core/test/image/testReadImage.cpp b/modules/core/test/image/testReadImage.cpp index a8e94381f4..d17eb1fb41 100644 --- a/modules/core/test/image/testReadImage.cpp +++ b/modules/core/test/image/testReadImage.cpp @@ -55,8 +55,7 @@ #define GETOPTARGS "cdi:p:h" void usage(const char *name, const char *badparam, std::string ipath); -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &ppath); +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath); /* @@ -109,8 +108,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &ppath) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath) { const char *optarg_; int c; @@ -182,8 +180,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } diff --git a/modules/core/test/image/testUndistortImage.cpp b/modules/core/test/image/testUndistortImage.cpp index ee460c5012..d6ff5860aa 100644 --- a/modules/core/test/image/testUndistortImage.cpp +++ b/modules/core/test/image/testUndistortImage.cpp @@ -72,8 +72,8 @@ \param user : Username. */ -void usage(const char *name, const char *badparam, const std::string &ipath, - const std::string &opath, const std::string &user) +void usage(const char *name, const char *badparam, const std::string &ipath, const std::string &opath, + const std::string &user) { fprintf(stdout, "\n\ Read an image from the disk, undistort it \n\ @@ -120,8 +120,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - std::string &opath, const std::string &user) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, const std::string &user) { const char *optarg_; int c; @@ -181,8 +180,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Set the default output path -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX opt_opath = "/tmp"; #elif defined(_WIN32) opt_opath = "C:\\temp"; @@ -214,8 +212,7 @@ int main(int argc, const char **argv) 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; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; exit(-1); } } @@ -226,8 +223,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -236,13 +232,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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); } @@ -280,16 +273,13 @@ int main(int argc, const char **argv) double endtime = vpTime::measureTimeMs(); - std::cout << "Time for 100 undistortion (ms): " << endtime - begintime - << std::endl; + 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")); + filename = vpIoTools::path(vpIoTools::createFilePath(opath, "Klimt_undistorted.pgm")); #elif defined COLOR - filename = vpIoTools::path( - vpIoTools::createFilePath(opath, "Klimt_undistorted.ppm")); + filename = vpIoTools::path(vpIoTools::createFilePath(opath, "Klimt_undistorted.ppm")); #endif std::cout << "Write undistorted image: " << filename << std::endl; vpImageIo::write(U, filename); diff --git a/modules/core/test/math/testArray2D.cpp b/modules/core/test/math/testArray2D.cpp index 84be1e36f8..2035a17721 100644 --- a/modules/core/test/math/testArray2D.cpp +++ b/modules/core/test/math/testArray2D.cpp @@ -48,21 +48,17 @@ #include -template -bool test(const std::string &s, const vpArray2D &A, - const std::vector &bench) +template bool test(const std::string &s, const vpArray2D &A, const std::vector &bench) { static unsigned int cpt = 0; std::cout << "** Test " << ++cpt << std::endl; - std::cout << s << "(" << A.getRows() << "," << A.getCols() << ") = \n" - << A << 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::epsilon()) { + if (std::fabs(A.data[i] - bench[i]) > std::fabs(A.data[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return false; } @@ -98,8 +94,7 @@ int main() vpArray2D B(A); if (test("B", B, bench) == false) return err; - std::cout << "Min/Max: " << B.getMinValue() << " " << B.getMaxValue() - << std::endl; + std::cout << "Min/Max: " << B.getMinValue() << " " << B.getMaxValue() << std::endl; } { // test constructor with initial value @@ -144,8 +139,7 @@ int main() vpArray2D B(A); if (test("B", B, bench) == false) return err; - std::cout << "Min/Max: " << B.getMinValue() << " " << B.getMaxValue() - << std::endl; + std::cout << "Min/Max: " << B.getMinValue() << " " << B.getMaxValue() << std::endl; } { // test constructor with initial value diff --git a/modules/core/test/math/testColVector.cpp b/modules/core/test/math/testColVector.cpp index c5bf47d248..fab5e427b6 100644 --- a/modules/core/test/math/testColVector.cpp +++ b/modules/core/test/math/testColVector.cpp @@ -51,20 +51,17 @@ namespace { -bool test(const std::string &s, const vpColVector &v, - const std::vector &bench) +bool test(const std::string &s, const vpColVector &v, const std::vector &bench) { static unsigned int cpt = 0; std::cout << "** Test " << ++cpt << std::endl; - std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" - << v.t() << "]^T" << 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::epsilon()) { + if (std::fabs(v[i] - bench[i]) > std::fabs(v[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return false; } @@ -371,14 +368,11 @@ int main() } t_sse = vpTime::measureTimeMs() - t_sse; - std::cout << "\nregular_sum=" << regular_sum << " ; sse_sum=" << sse_sum - << std::endl; - std::cout << "t_regular=" << t_regular << " ms ; t_sse=" << t_sse << " ms" - << std::endl; + std::cout << "\nregular_sum=" << regular_sum << " ; sse_sum=" << sse_sum << std::endl; + std::cout << "t_regular=" << t_regular << " ms ; t_sse=" << t_sse << " ms" << std::endl; std::cout << "Speed-up: " << (t_regular / t_sse) << "X" << std::endl; - if (!vpMath::equal(regular_sum, sse_sum, - std::numeric_limits::epsilon())) { + if (!vpMath::equal(regular_sum, sse_sum, std::numeric_limits::epsilon())) { std::cerr << "Problem when computing v.sum()!" << std::endl; return EXIT_FAILURE; } @@ -397,14 +391,11 @@ int main() } t_sse = vpTime::measureTimeMs() - t_sse; - std::cout << "\nregular_sumSquare=" << regular_sumSquare - << " ; sse_sumSquare=" << sse_sumSquare << std::endl; - std::cout << "t_regular=" << t_regular << " ms ; t_sse=" << t_sse << " ms" - << std::endl; + std::cout << "\nregular_sumSquare=" << regular_sumSquare << " ; sse_sumSquare=" << sse_sumSquare << std::endl; + std::cout << "t_regular=" << t_regular << " ms ; t_sse=" << t_sse << " ms" << std::endl; std::cout << "Speed-up: " << (t_regular / t_sse) << "X" << std::endl; - if (!vpMath::equal(regular_sumSquare, sse_sumSquare, - std::numeric_limits::epsilon())) { + if (!vpMath::equal(regular_sumSquare, sse_sumSquare, std::numeric_limits::epsilon())) { std::cerr << "Problem when computing v.sumSquare()!" << std::endl; return EXIT_FAILURE; } @@ -423,16 +414,12 @@ int main() } t_sse = vpTime::measureTimeMs() - t_sse; - std::cout << "\nregular_stdev=" << regular_stdev - << " ; sse_stdev=" << sse_stdev << std::endl; - std::cout << "t_regular=" << t_regular << " ms ; t_sse=" << t_sse << " ms" - << std::endl; + std::cout << "\nregular_stdev=" << regular_stdev << " ; sse_stdev=" << sse_stdev << std::endl; + std::cout << "t_regular=" << t_regular << " ms ; t_sse=" << t_sse << " ms" << std::endl; std::cout << "Speed-up: " << (t_regular / t_sse) << "X" << std::endl; - if (!vpMath::equal(regular_stdev, sse_stdev, - std::numeric_limits::epsilon())) { - std::cerr << "Problem when computing vpColVector::stdev()!" - << std::endl; + if (!vpMath::equal(regular_stdev, sse_stdev, std::numeric_limits::epsilon())) { + std::cerr << "Problem when computing vpColVector::stdev()!" << std::endl; return EXIT_FAILURE; } } @@ -461,8 +448,7 @@ int main() for (unsigned int i = 0; i < nb; i++) { for (unsigned int j = 0; j < size; j++) { - if (!vpMath::equal(v_big[i * size + j], vec[(size_t)i][j], - std::numeric_limits::epsilon())) { + if (!vpMath::equal(v_big[i * size + j], vec[(size_t)i][j], std::numeric_limits::epsilon())) { std::cerr << "Problem in vpColVector insert()!" << std::endl; return EXIT_FAILURE; } diff --git a/modules/core/test/math/testKalmanAcceleration.cpp b/modules/core/test/math/testKalmanAcceleration.cpp index eb300c140e..39ce258243 100644 --- a/modules/core/test/math/testKalmanAcceleration.cpp +++ b/modules/core/test/math/testKalmanAcceleration.cpp @@ -59,8 +59,7 @@ int main() vpLinearKalmanFilterInstantiation kalman; vpLinearKalmanFilterInstantiation::vpStateModel model; - model = vpLinearKalmanFilterInstantiation:: - stateConstAccWithColoredNoise_MeasureVel; + model = vpLinearKalmanFilterInstantiation::stateConstAccWithColoredNoise_MeasureVel; kalman.setStateModel(model); unsigned int size_state_vector = kalman.getStateSize() * nsignal; @@ -90,8 +89,7 @@ int main() 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)); + velocity_measure[signal] = 3 + 2 * signal + 0.3 * sin(vpMath::rad(360. / niter * iter)); } std::cout << "measure : " << velocity_measure.t() << std::endl; diff --git a/modules/core/test/math/testKalmanVelocity.cpp b/modules/core/test/math/testKalmanVelocity.cpp index d2ed8124f7..aa5d23fe5a 100644 --- a/modules/core/test/math/testKalmanVelocity.cpp +++ b/modules/core/test/math/testKalmanVelocity.cpp @@ -101,8 +101,7 @@ int main() double dummy = 0; // non used parameter switch (measure_t) { case Velocity: - model = vpLinearKalmanFilterInstantiation:: - stateConstVelWithColoredNoise_MeasureVel; + model = vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel; kalman.setStateModel(model); kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy); break; @@ -116,8 +115,7 @@ int main() 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)); + measure[signal] = 3 + 2 * signal + 0.3 * sin(vpMath::rad(360. / niter * iter)); } std::cout << "measure : " << measure.t() << std::endl; diff --git a/modules/core/test/math/testMath.cpp b/modules/core/test/math/testMath.cpp index 34ab551d56..c009a781aa 100644 --- a/modules/core/test/math/testMath.cpp +++ b/modules/core/test/math/testMath.cpp @@ -67,72 +67,61 @@ int main() { // Test isNaN if (vpMath::isNaN(0.0)) { - std::cerr << "Fail: IsNaN(0.0)=" << vpMath::isNaN(0.0) - << " / should be false" << std::endl; + 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; + 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; + 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; + 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::quiet_NaN())) { - std::cerr << "Fail: IsNaN(quiet_NaN)=" - << vpMath::isNaN(std::numeric_limits::quiet_NaN()) + std::cerr << "Fail: IsNaN(quiet_NaN)=" << vpMath::isNaN(std::numeric_limits::quiet_NaN()) << " / should be true" << std::endl; return -1; } if (!vpMath::isNaN(std::numeric_limits::signaling_NaN())) { - std::cerr << "Fail: IsNaN(signaling_NaN)=" - << vpMath::isNaN(std::numeric_limits::signaling_NaN()) + std::cerr << "Fail: IsNaN(signaling_NaN)=" << vpMath::isNaN(std::numeric_limits::signaling_NaN()) << " / should be true" << std::endl; return -1; } if (vpMath::isNaN(std::numeric_limits::infinity())) { - std::cerr << "Fail: IsNaN(infinity)=" - << vpMath::isNaN(std::numeric_limits::infinity()) + std::cerr << "Fail: IsNaN(infinity)=" << vpMath::isNaN(std::numeric_limits::infinity()) << " / should be false" << std::endl; return -1; } if (vpMath::isNaN(1.0 / std::numeric_limits::epsilon())) { - std::cerr << "Fail: IsNaN(1.0/epsilon)=" - << vpMath::isNaN(1.0 / std::numeric_limits::epsilon()) + std::cerr << "Fail: IsNaN(1.0/epsilon)=" << vpMath::isNaN(1.0 / std::numeric_limits::epsilon()) << " / should be false" << std::endl; return -1; } - if (!vpMath::isNaN(std::numeric_limits::infinity() - - std::numeric_limits::infinity())) { + if (!vpMath::isNaN(std::numeric_limits::infinity() - std::numeric_limits::infinity())) { std::cerr << "Fail: IsNaN(infinity - infinity)=" - << vpMath::isNaN(std::numeric_limits::infinity() - - std::numeric_limits::infinity()) + << vpMath::isNaN(std::numeric_limits::infinity() - std::numeric_limits::infinity()) << " / 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; + 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; @@ -142,33 +131,28 @@ int main() // 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; + 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; + 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; + 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; + 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::cerr << "Fail: vpMath::isInf(DBL_MIN/2.0)=" << vpMath::isInf(DBL_MIN / 2.0) << " / should be false" << std::endl; return -1; } @@ -176,44 +160,37 @@ int main() // Test round if (vpMath::round(2.3) != 2) { - std::cerr << "Fail: vpMath::round(2.3)=" << vpMath::round(2.3) - << " / should be 2" << std::endl; + 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; + 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; + 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; + 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; + 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; + 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; + 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; @@ -223,88 +200,83 @@ int main() char char_value = -127; unsigned char uchar_value = vpMath::saturate(char_value); if (uchar_value != 0) { - std::cerr << "Fail: vpMath::saturate(-127)=" << uchar_value - << " / should be 0" << std::endl; + std::cerr << "Fail: vpMath::saturate(-127)=" << uchar_value << " / should be 0" << std::endl; return -1; } unsigned short ushort_value = 60000; uchar_value = vpMath::saturate(ushort_value); if (uchar_value != UCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(60000)=" - << uchar_value << " / should be " << UCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(60000)=" << uchar_value << " / should be " << UCHAR_MAX + << std::endl; return -1; } int int_value = 70000; uchar_value = vpMath::saturate(int_value); if (uchar_value != UCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(70000)=" - << uchar_value << " / should be " << UCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(70000)=" << uchar_value << " / should be " << UCHAR_MAX + << std::endl; return -1; } int_value = -70000; uchar_value = vpMath::saturate(int_value); if (uchar_value != 0) { - std::cerr << "Fail: vpMath::saturate(-70000)=" - << uchar_value << " / should be 0" << std::endl; + std::cerr << "Fail: vpMath::saturate(-70000)=" << uchar_value << " / should be 0" << std::endl; return -1; } short short_value = 30000; uchar_value = vpMath::saturate(short_value); if (uchar_value != UCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(30000)=" - << uchar_value << " / should be " << UCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(30000)=" << uchar_value << " / should be " << UCHAR_MAX + << std::endl; return -1; } short_value = -30000; uchar_value = vpMath::saturate(short_value); if (uchar_value != 0) { - std::cerr << "Fail: vpMath::saturate(-30000)=" - << uchar_value << " / should be 0" << std::endl; + std::cerr << "Fail: vpMath::saturate(-30000)=" << uchar_value << " / should be 0" << std::endl; return -1; } unsigned int uint_value = 10000; uchar_value = vpMath::saturate(uint_value); if (uchar_value != UCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(10000)=" - << uchar_value << " / should be " << UCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(10000)=" << uchar_value << " / should be " << UCHAR_MAX + << std::endl; return -1; } float float_value = 10000.1f; uchar_value = vpMath::saturate(float_value); if (uchar_value != UCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(10000.1f)=" - << uchar_value << " / should be " << UCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(10000.1f)=" << uchar_value << " / should be " << UCHAR_MAX + << std::endl; return -1; } float_value = -10000.1f; uchar_value = vpMath::saturate(float_value); if (uchar_value != 0) { - std::cerr << "Fail: vpMath::saturate(-10000.1f)=" - << uchar_value << " / should be 0" << std::endl; + std::cerr << "Fail: vpMath::saturate(-10000.1f)=" << uchar_value << " / should be 0" << std::endl; return -1; } double double_value = 10000.1; uchar_value = vpMath::saturate(double_value); if (uchar_value != UCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(10000.0)=" - << uchar_value << " / should be " << UCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(10000.0)=" << uchar_value << " / should be " << UCHAR_MAX + << std::endl; return -1; } double_value = -10000.1; uchar_value = vpMath::saturate(double_value); if (uchar_value != 0) { - std::cerr << "Fail: vpMath::saturate(-10000.0)=" - << uchar_value << " / should be 0" << std::endl; + std::cerr << "Fail: vpMath::saturate(-10000.0)=" << uchar_value << " / should be 0" << std::endl; return -1; } std::cout << "vpMath::saturate() is Ok !" << std::endl; @@ -313,88 +285,77 @@ int main() uchar_value = 255; char_value = vpMath::saturate(uchar_value); if (char_value != SCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(255)=" << char_value - << " / should be " << SCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(255)=" << char_value << " / should be " << SCHAR_MAX << std::endl; return -1; } ushort_value = 60000; char_value = vpMath::saturate(ushort_value); if (char_value != SCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(60000)=" << char_value - << " / should be " << SCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(60000)=" << char_value << " / should be " << SCHAR_MAX << std::endl; return -1; } int_value = 70000; char_value = vpMath::saturate(int_value); if (char_value != SCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(70000)=" << char_value - << " / should be " << SCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(70000)=" << char_value << " / should be " << SCHAR_MAX << std::endl; return -1; } int_value = -70000; char_value = vpMath::saturate(int_value); if (char_value != (char)SCHAR_MIN) { - std::cerr << "Fail: vpMath::saturate(-70000)=" << char_value - << " / should be " << SCHAR_MIN << std::endl; + std::cerr << "Fail: vpMath::saturate(-70000)=" << char_value << " / should be " << SCHAR_MIN << std::endl; return -1; } short_value = 30000; char_value = vpMath::saturate(short_value); if (char_value != SCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(30000)=" << char_value - << " / should be " << SCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(30000)=" << char_value << " / should be " << SCHAR_MAX << std::endl; return -1; } short_value = -30000; char_value = vpMath::saturate(short_value); if (char_value != (char)SCHAR_MIN) { - std::cerr << "Fail: vpMath::saturate(-30000)=" << char_value - << " / should be " << SCHAR_MIN << std::endl; + std::cerr << "Fail: vpMath::saturate(-30000)=" << char_value << " / should be " << SCHAR_MIN << std::endl; return -1; } uint_value = 10000; char_value = vpMath::saturate(uint_value); if (char_value != SCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(10000)=" << char_value - << " / should be " << SCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(10000)=" << char_value << " / should be " << SCHAR_MAX << std::endl; return -1; } float_value = 10000.1f; char_value = vpMath::saturate(float_value); if (char_value != SCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(10000.1f)=" << char_value - << " / should be " << SCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(10000.1f)=" << char_value << " / should be " << SCHAR_MAX << std::endl; return -1; } float_value = -10000.1f; char_value = vpMath::saturate(float_value); if (char_value != (char)SCHAR_MIN) { - std::cerr << "Fail: vpMath::saturate(-10000.1f)=" << char_value - << " / should be " << SCHAR_MIN << std::endl; + std::cerr << "Fail: vpMath::saturate(-10000.1f)=" << char_value << " / should be " << SCHAR_MIN << std::endl; return -1; } double_value = 10000.1; char_value = vpMath::saturate(double_value); if (char_value != SCHAR_MAX) { - std::cerr << "Fail: vpMath::saturate(10000.1)=" << char_value - << " / should be " << SCHAR_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(10000.1)=" << char_value << " / should be " << SCHAR_MAX << std::endl; return -1; } double_value = -10000.1; char_value = vpMath::saturate(double_value); if (char_value != (char)SCHAR_MIN) { - std::cerr << "Fail: vpMath::saturate(-10000.1)=" << char_value - << " / should be " << SCHAR_MIN << std::endl; + std::cerr << "Fail: vpMath::saturate(-10000.1)=" << char_value << " / should be " << SCHAR_MIN << std::endl; return -1; } std::cout << "vpMath::saturate() is Ok !" << std::endl; @@ -403,72 +364,67 @@ int main() char_value = -127; ushort_value = vpMath::saturate(char_value); if (ushort_value != 0) { - std::cerr << "Fail: vpMath::saturate(-127)=" - << ushort_value << " / should be 0" << std::endl; + std::cerr << "Fail: vpMath::saturate(-127)=" << ushort_value << " / should be 0" << std::endl; return -1; } short_value = -30000; ushort_value = vpMath::saturate(short_value); if (ushort_value != 0) { - std::cerr << "Fail: vpMath::saturate(-30000)=" - << ushort_value << " / should be 0" << std::endl; + std::cerr << "Fail: vpMath::saturate(-30000)=" << ushort_value << " / should be 0" << std::endl; return -1; } int_value = 70000; ushort_value = vpMath::saturate(int_value); if (ushort_value != USHRT_MAX) { - std::cerr << "Fail: vpMath::saturate(70000)=" - << ushort_value << " / should be " << USHRT_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(70000)=" << ushort_value << " / should be " << USHRT_MAX + << std::endl; return -1; } int_value = -70000; ushort_value = vpMath::saturate(int_value); if (ushort_value != 0) { - std::cerr << "Fail: vpMath::saturate(-70000)=" - << ushort_value << " / should be 0" << std::endl; + std::cerr << "Fail: vpMath::saturate(-70000)=" << ushort_value << " / should be 0" << std::endl; return -1; } uint_value = 70000; ushort_value = vpMath::saturate(uint_value); if (ushort_value != USHRT_MAX) { - std::cerr << "Fail: vpMath::saturate(70000)=" - << ushort_value << " / should be " << USHRT_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(70000)=" << ushort_value << " / should be " << USHRT_MAX + << std::endl; return -1; } float_value = 70000.1f; ushort_value = vpMath::saturate(float_value); if (ushort_value != USHRT_MAX) { - std::cerr << "Fail: vpMath::saturate(70000.1f)=" - << ushort_value << " / should be " << USHRT_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(70000.1f)=" << ushort_value << " / should be " << USHRT_MAX + << std::endl; return -1; } float_value = -10000.1f; ushort_value = vpMath::saturate(float_value); if (ushort_value != 0) { - std::cerr << "Fail: vpMath::saturate(-10000.1f)=" - << ushort_value << " / should be 0" << std::endl; + std::cerr << "Fail: vpMath::saturate(-10000.1f)=" << ushort_value << " / should be 0" << std::endl; return -1; } double_value = 70000.1; ushort_value = vpMath::saturate(double_value); if (ushort_value != USHRT_MAX) { - std::cerr << "Fail: vpMath::saturate(70000.1)=" - << ushort_value << " / should be " << USHRT_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(70000.1)=" << ushort_value << " / should be " << USHRT_MAX + << std::endl; return -1; } double_value = -10000.1; ushort_value = vpMath::saturate(double_value); if (ushort_value != 0) { - std::cerr << "Fail: vpMath::saturate(-10000.1)=" - << ushort_value << " / should be 0" << std::endl; + std::cerr << "Fail: vpMath::saturate(-10000.1)=" << ushort_value << " / should be 0" << std::endl; return -1; } std::cout << "vpMath::saturate() is Ok !" << std::endl; @@ -477,64 +433,56 @@ int main() ushort_value = 60000; short_value = vpMath::saturate(ushort_value); if (short_value != SHRT_MAX) { - std::cerr << "Fail: vpMath::saturate(60000)=" << short_value - << " / should be " << SHRT_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(60000)=" << short_value << " / should be " << SHRT_MAX << std::endl; return -1; } int_value = 70000; short_value = vpMath::saturate(int_value); if (short_value != SHRT_MAX) { - std::cerr << "Fail: vpMath::saturate(70000)=" << short_value - << " / should be " << SHRT_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(70000)=" << short_value << " / should be " << SHRT_MAX << std::endl; return -1; } int_value = -70000; short_value = vpMath::saturate(int_value); if (short_value != SHRT_MIN) { - std::cerr << "Fail: vpMath::saturate(-70000)=" << short_value - << " / should be " << SHRT_MIN << std::endl; + std::cerr << "Fail: vpMath::saturate(-70000)=" << short_value << " / should be " << SHRT_MIN << std::endl; return -1; } uint_value = 70000; short_value = vpMath::saturate(uint_value); if (short_value != SHRT_MAX) { - std::cerr << "Fail: vpMath::saturate(70000)=" << short_value - << " / should be " << SHRT_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(70000)=" << short_value << " / should be " << SHRT_MAX << std::endl; return -1; } float_value = 70000.1f; short_value = vpMath::saturate(float_value); if (short_value != SHRT_MAX) { - std::cerr << "Fail: vpMath::saturate(70000.1f)=" << short_value - << " / should be " << SHRT_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(70000.1f)=" << short_value << " / should be " << SHRT_MAX << std::endl; return -1; } float_value = -70000.1f; short_value = vpMath::saturate(float_value); if (short_value != SHRT_MIN) { - std::cerr << "Fail: vpMath::saturate(-70000.1f)=" << short_value - << " / should be " << SHRT_MIN << std::endl; + std::cerr << "Fail: vpMath::saturate(-70000.1f)=" << short_value << " / should be " << SHRT_MIN << std::endl; return -1; } double_value = 70000.1; short_value = vpMath::saturate(double_value); if (short_value != SHRT_MAX) { - std::cerr << "Fail: vpMath::saturate(70000.1)=" << short_value - << " / should be " << SHRT_MAX << std::endl; + std::cerr << "Fail: vpMath::saturate(70000.1)=" << short_value << " / should be " << SHRT_MAX << std::endl; return -1; } double_value = -70000.1; short_value = vpMath::saturate(double_value); if (short_value != SHRT_MIN) { - std::cerr << "Fail: vpMath::saturate(70000.1)=" << short_value - << " / should be " << SHRT_MIN << std::endl; + std::cerr << "Fail: vpMath::saturate(70000.1)=" << short_value << " / should be " << SHRT_MIN << std::endl; return -1; } std::cout << "vpMath::saturate() is Ok !" << std::endl; @@ -568,8 +516,7 @@ int main() 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; + std::cerr << "Problem with vpMath::getStdev() with Bessel correction=" << res << std::endl; return -1; } std::cout << "vpMath::getStdev() is Ok !" << std::endl; diff --git a/modules/core/test/math/testMatrix.cpp b/modules/core/test/math/testMatrix.cpp index 7b9b95af40..a003784f21 100644 --- a/modules/core/test/math/testMatrix.cpp +++ b/modules/core/test/math/testMatrix.cpp @@ -54,20 +54,17 @@ namespace { -bool test(const std::string &s, const vpMatrix &M, - const std::vector &bench) +bool test(const std::string &s, const vpMatrix &M, const std::vector &bench) { static unsigned int cpt = 0; std::cout << "** Test " << ++cpt << std::endl; - std::cout << s << "(" << M.getRows() << "," << M.getCols() << ") = \n" - << M << 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::epsilon()) { + if (std::fabs(M.data[i] - bench[i]) > std::fabs(M.data[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return false; } @@ -81,8 +78,7 @@ double getRandomValues(const double min, const double max) return (max - min) * ((double)rand() / (double)RAND_MAX) + min; } -bool equalMatrix(const vpMatrix &A, const vpMatrix &B, - const double tol = std::numeric_limits::epsilon()) +bool equalMatrix(const vpMatrix &A, const vpMatrix &B, const double tol = std::numeric_limits::epsilon()) { if (A.getRows() != B.getRows() || A.getCols() != B.getCols()) { return false; @@ -100,9 +96,7 @@ bool equalMatrix(const vpMatrix &A, const vpMatrix &B, } #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) -vpMatrix generateRandomMatrix(const unsigned int rows, - const unsigned int cols, const double min, - const double max) +vpMatrix generateRandomMatrix(const unsigned int rows, const unsigned int cols, const double min, const double max) { vpMatrix M(rows, cols); @@ -115,8 +109,7 @@ vpMatrix generateRandomMatrix(const unsigned int rows, return M; } -vpColVector generateRandomVector(const unsigned int rows, const double min, - const double max) +vpColVector generateRandomVector(const unsigned int rows, const double min, const double max) { vpColVector v(rows); @@ -136,9 +129,8 @@ vpMatrix dgemm_regular(const vpMatrix &A, const vpMatrix &B) C.resize(A.getRows(), B.getCols(), false); if (A.getCols() != B.getRows()) { - throw(vpException(vpException::dimensionError, - "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", - A.getRows(), A.getCols(), B.getRows(), B.getCols())); + 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 @@ -198,10 +190,8 @@ vpMatrix dgemv_regular(const vpMatrix &A, const vpColVector &v) vpColVector w; if (A.getCols() != v.getRows()) { - throw(vpException( - vpException::dimensionError, - "Cannot multiply a (%dx%d) matrix by a (%d) column vector", - A.getRows(), A.getCols(), v.getRows())); + throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector", + A.getRows(), A.getCols(), v.getRows())); } w.resize(A.getRows(), true); @@ -217,16 +207,13 @@ vpMatrix dgemv_regular(const vpMatrix &A, const vpColVector &v) } // Copy of vpMatrix::operator*(const vpVelocityTwistMatrix &V) -vpMatrix mat_mul_twist_matrix(const vpMatrix &A, - const vpVelocityTwistMatrix &V) +vpMatrix mat_mul_twist_matrix(const vpMatrix &A, const vpVelocityTwistMatrix &V) { vpMatrix M; if (A.getCols() != V.getRows()) { - throw(vpException( - vpException::dimensionError, - "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix", - A.getRows(), A.getCols())); + throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix", + A.getRows(), A.getCols())); } M.resize(A.getRows(), 6, false); @@ -292,9 +279,7 @@ int main() vpMatrix M1; char header_[100]; if (vpMatrix::loadMatrix("matrix.mat", M1, false, header_)) - std::cout << "Matrix loaded from matrix.mat file with header \"" - << header_ << "\": \n" - << M1 << std::endl; + std::cout << "Matrix loaded from matrix.mat file with header \"" << header_ << "\": \n" << M1 << std::endl; else return err; if (header != std::string(header_)) { @@ -310,9 +295,7 @@ int main() // Load matrix in binary format if (vpMatrix::loadMatrix("matrix.bin", M1, true, header_)) - std::cout << "Matrix loaded from matrix.bin file with header \"" - << header_ << "\": \n" - << M1 << std::endl; + std::cout << "Matrix loaded from matrix.bin file with header \"" << header_ << "\": \n" << M1 << std::endl; else return err; if (header != std::string(header_)) { @@ -329,9 +312,7 @@ int main() // Read matrix in YAML format vpMatrix M2; if (vpMatrix::loadMatrixYAML("matrix.yml", M2, header_)) - std::cout << "Matrix loaded from matrix.yml file with header \"" - << header_ << "\": \n" - << M2 << std::endl; + std::cout << "Matrix loaded from matrix.yml file with header \"" << header_ << "\": \n" << M2 << std::endl; else return err; if (header != std::string(header_)) { @@ -488,8 +469,7 @@ int main() { std::cout << "------------------------" << std::endl; - std::cout << "--- TEST vpMatrix insert() with same colNum " - << std::endl; + std::cout << "--- TEST vpMatrix insert() with same colNum " << std::endl; std::cout << "------------------------" << std::endl; const unsigned int nb = 100; // 10000; //for ctest otherwise takes too // long time with static call @@ -519,8 +499,7 @@ int main() for (unsigned int cpt = 0; cpt < nb; cpt++) { for (unsigned int i = 0; i < size; i++) { for (unsigned int j = 0; j < 6; j++) { - if (!vpMath::equal(m_big[cpt * size + i][j], - submatrices[(size_t)cpt][i][j], + if (!vpMath::equal(m_big[cpt * size + i][j], submatrices[(size_t)cpt][i][j], std::numeric_limits::epsilon())) { std::cerr << "Problem with vpMatrix insert()!" << std::endl; return EXIT_FAILURE; @@ -603,8 +582,7 @@ int main() vpMatrix m_big_stack_static, m_big_stack_static_tmp; t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < nb; i++) { - vpMatrix::stack(m_big_stack_static_tmp, submatrices[(size_t)i], - m_big_stack_static); + vpMatrix::stack(m_big_stack_static_tmp, submatrices[(size_t)i], m_big_stack_static); m_big_stack_static_tmp = m_big_stack_static; } t = vpTime::measureTimeMs() - t; @@ -616,20 +594,17 @@ int main() } std::cout << "------------------------" << std::endl; - std::cout << "--- TEST vpMatrix::stack(vpMatrix, vpRowVector, vpMatrix)" - << std::endl; + std::cout << "--- TEST vpMatrix::stack(vpMatrix, vpRowVector, vpMatrix)" << std::endl; std::cout << "------------------------" << std::endl; vpMatrix m_big_stack_static_row, m_big_stack_static_row_tmp; t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < m_big_stack_static.getRows(); i++) { - vpMatrix::stack(m_big_stack_static_row_tmp, - m_big_stack_static.getRow(i), m_big_stack_static_row); + vpMatrix::stack(m_big_stack_static_row_tmp, m_big_stack_static.getRow(i), m_big_stack_static_row); m_big_stack_static_row_tmp = m_big_stack_static_row; } t = vpTime::measureTimeMs() - t; - std::cout << "\nMatrix::stack(vpMatrix, vpRowVector, vpMatrix): " << t - << " ms" << std::endl; + std::cout << "\nMatrix::stack(vpMatrix, vpRowVector, vpMatrix): " << t << " ms" << std::endl; if (!equalMatrix(m_big_stack_static, m_big_stack_static_row)) { std::cerr << "Problem with vpMatrix::stack(vpMatrix, vpRowVector, " @@ -652,8 +627,7 @@ int main() for (unsigned int i = 0; i < m2.getRows(); i++) { for (unsigned int j = 0; j < m2.getCols(); j++) { - if (!vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], - std::numeric_limits::epsilon())) { + if (!vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits::epsilon())) { std::cerr << "Problem with vpMatrix insert()!" << std::endl; return EXIT_FAILURE; } @@ -665,8 +639,7 @@ int main() for (unsigned int i = 0; i < m2.getRows(); i++) { for (unsigned int j = 0; j < m2.getCols(); j++) { - if (!vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], - std::numeric_limits::epsilon())) { + if (!vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits::epsilon())) { std::cerr << "Problem with vpMatrix insert()!" << std::endl; return EXIT_FAILURE; } @@ -678,8 +651,7 @@ int main() for (unsigned int i = 0; i < m2.getRows(); i++) { for (unsigned int j = 0; j < m2.getCols(); j++) { - if (!vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], - std::numeric_limits::epsilon())) { + if (!vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits::epsilon())) { std::cerr << "Problem with vpMatrix insert()!" << std::endl; return EXIT_FAILURE; } @@ -731,8 +703,7 @@ int main() vec_C.push_back(vec_A[i] * vec_B[i]); } t = vpTime::measureTimeMs() - t; - std::cout << nb_matrices << " matrix multiplication: (6x200) x (200x6)" - << std::endl; + std::cout << nb_matrices << " matrix multiplication: (6x200) x (200x6)" << std::endl; std::cout << "Lapack: " << t << " ms" << std::endl; std::cout << "vec_C:\n" << vec_C.back() << std::endl; @@ -744,25 +715,21 @@ int main() std::cout << "\nRegular: " << t << " ms" << std::endl; std::cout << "vec_C_regular:\n" << vec_C_regular.back() << std::endl; - vpMatrix A = generateRandomMatrix(480, 640, min, max), - B = generateRandomMatrix(640, 480, min, max); + vpMatrix A = generateRandomMatrix(480, 640, min, max), B = generateRandomMatrix(640, 480, min, max); vpMatrix AB, AB_regular; t = vpTime::measureTimeMs(); AB = A * B; t = vpTime::measureTimeMs() - t; - std::cout << "\nMatrix multiplication: (480x640) x (640x480)" - << std::endl; + std::cout << "\nMatrix multiplication: (480x640) x (640x480)" << std::endl; std::cout << "Lapack: " << t << " ms" << std::endl; - std::cout << "Min=" << AB.getMinValue() << " ; Max=" << AB.getMaxValue() - << std::endl; + std::cout << "Min=" << AB.getMinValue() << " ; Max=" << AB.getMaxValue() << std::endl; t = vpTime::measureTimeMs(); AB_regular = dgemm_regular(A, B); t = vpTime::measureTimeMs() - t; std::cout << "Regular: " << t << " ms" << std::endl; - std::cout << "Min=" << AB_regular.getMinValue() - << " ; Max=" << AB_regular.getMaxValue() << std::endl; + std::cout << "Min=" << AB_regular.getMinValue() << " ; Max=" << AB_regular.getMaxValue() << std::endl; bool res = equalMatrix(AB, AB_regular, 1e-9); std::cout << "Check result: " << res << std::endl; if (!res) { @@ -778,9 +745,7 @@ int main() for (int i = 0; i < nb_iterations; i++) LTL = L.AtA(); t = vpTime::measureTimeMs() - t; - std::cout << "\n" - << nb_iterations << " iterations of AtA for size: (1000x6)" - << std::endl; + std::cout << "\n" << nb_iterations << " iterations of AtA for size: (1000x6)" << std::endl; std::cout << "Lapack: " << t << " ms" << std::endl; std::cout << "LTL:\n" << LTL << std::endl; @@ -826,10 +791,8 @@ int main() return EXIT_FAILURE; } - vpVelocityTwistMatrix V( - getRandomValues(min, max), getRandomValues(min, max), - getRandomValues(min, max), getRandomValues(min, max), - getRandomValues(min, max), getRandomValues(min, max)); + vpVelocityTwistMatrix V(getRandomValues(min, max), getRandomValues(min, max), getRandomValues(min, max), + getRandomValues(min, max), getRandomValues(min, max), getRandomValues(min, max)); vpMatrix LV, LV_regular; t = vpTime::measureTimeMs(); @@ -851,9 +814,7 @@ int main() res = equalMatrix(LV, LV_regular, 1e-9); std::cout << "Check result: " << res << std::endl; if (!res) { - std::cerr - << "Problem with matrix and velocity twist matrix multiplication!" - << std::endl; + std::cerr << "Problem with matrix and velocity twist matrix multiplication!" << std::endl; return EXIT_FAILURE; } } diff --git a/modules/core/test/math/testMatrixDeterminant.cpp b/modules/core/test/math/testMatrixDeterminant.cpp index 4bc5227679..c772112720 100644 --- a/modules/core/test/math/testMatrixDeterminant.cpp +++ b/modules/core/test/math/testMatrixDeterminant.cpp @@ -111,10 +111,8 @@ OPTIONS: Default\n\ \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) +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; @@ -181,13 +179,11 @@ vpMatrix make_random_matrix(unsigned int nbrows, unsigned int nbcols) return A; } -void create_bench(unsigned int nb_matrices, unsigned int nb_rows, - unsigned int nb_cols, bool verbose, +void create_bench(unsigned int nb_matrices, unsigned int nb_rows, unsigned int nb_cols, bool verbose, std::vector &bench) { if (verbose) - std::cout << "Create a bench of " << nb_matrices << " " << nb_rows - << " by " << nb_cols << " matrices" << std::endl; + std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_cols << " matrices" << std::endl; bench.clear(); for (unsigned int i = 0; i < nb_matrices; i++) { vpMatrix M = make_random_matrix(nb_rows, nb_cols); @@ -195,15 +191,13 @@ void create_bench(unsigned int nb_matrices, unsigned int nb_rows, } } -void test_det_default(bool verbose, const std::vector &bench, - double &time, std::vector &result) +void test_det_default(bool verbose, const std::vector &bench, double &time, std::vector &result) { if (verbose) std::cout << "Test determinant using default method" << std::endl; // Compute inverse if (verbose) - std::cout << " Matrix size: " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() << std::endl; + std::cout << " Matrix size: " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << std::endl; result.resize(bench.size()); double t = vpTime::measureTimeMs(); @@ -214,15 +208,13 @@ void test_det_default(bool verbose, const std::vector &bench, } #if defined(VISP_HAVE_EIGEN3) -void test_det_eigen3(bool verbose, const std::vector &bench, - double &time, std::vector &result) +void test_det_eigen3(bool verbose, const std::vector &bench, double &time, std::vector &result) { if (verbose) std::cout << "Test determinant using Eigen3 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " Matrix size: " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() << std::endl; + std::cout << " Matrix size: " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << std::endl; result.resize(bench.size()); double t = vpTime::measureTimeMs(); @@ -234,15 +226,13 @@ void test_det_eigen3(bool verbose, const std::vector &bench, #endif #if defined(VISP_HAVE_GSL) -void test_det_gsl(bool verbose, const std::vector &bench, - double &time, std::vector &result) +void test_det_gsl(bool verbose, const std::vector &bench, double &time, std::vector &result) { if (verbose) std::cout << "Test determinant using GSL 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " Matrix size: " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() << std::endl; + std::cout << " Matrix size: " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << std::endl; result.resize(bench.size()); double t = vpTime::measureTimeMs(); @@ -254,15 +244,13 @@ void test_det_gsl(bool verbose, const std::vector &bench, #endif #if defined(VISP_HAVE_LAPACK) -void test_det_lapack(bool verbose, const std::vector &bench, - double &time, std::vector &result) +void test_det_lapack(bool verbose, const std::vector &bench, double &time, std::vector &result) { if (verbose) std::cout << "Test determinant using Lapack 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " Matrix size: " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() << std::endl; + std::cout << " Matrix size: " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << std::endl; result.resize(bench.size()); double t = vpTime::measureTimeMs(); @@ -274,15 +262,13 @@ void test_det_lapack(bool verbose, const std::vector &bench, #endif #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) -void test_det_opencv(bool verbose, const std::vector &bench, - double &time, std::vector &result) +void test_det_opencv(bool verbose, const std::vector &bench, double &time, std::vector &result) { if (verbose) std::cout << "Test determinant using OpenCV 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " Matrix size: " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() << std::endl; + std::cout << " Matrix size: " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << std::endl; result.resize(bench.size()); double t = vpTime::measureTimeMs(); @@ -293,8 +279,7 @@ void test_det_opencv(bool verbose, const std::vector &bench, } #endif -void save_time(const std::string &method, bool verbose, bool use_plot_file, - std::ofstream &of, double time) +void save_time(const std::string &method, bool verbose, bool use_plot_file, std::ofstream &of, double time) { if (use_plot_file) of << time << "\t"; @@ -306,8 +291,8 @@ void save_time(const std::string &method, bool verbose, bool use_plot_file, int main(int argc, const char *argv[]) { try { -#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_GSL) || \ - defined(VISP_HAVE_LAPACK) || (VISP_HAVE_OPENCV_VERSION >= 0x020101) +#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_GSL) || defined(VISP_HAVE_LAPACK) || \ + (VISP_HAVE_OPENCV_VERSION >= 0x020101) unsigned int nb_matrices = 1000; unsigned int nb_iterations = 10; unsigned int nb_rows = 6; @@ -318,8 +303,8 @@ int main(int argc, const char *argv[]) std::ofstream of; // Read the command line options - if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, - plotfile, nb_rows, nb_cols, verbose) == false) { + if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, plotfile, nb_rows, nb_cols, verbose) == + false) { exit(-1); } @@ -395,8 +380,8 @@ int main(int argc, const char *argv[]) // Compare results for (unsigned int i = 0; i < bench.size(); i++) { if (std::fabs(result_gsl[i] - result_opencv[i]) > 1e-6) { - std::cout << "Determinant differ between GSL and OpenCV: " - << result_gsl[i] << " " << result_opencv[i] << std::endl; + std::cout << "Determinant differ between GSL and OpenCV: " << result_gsl[i] << " " << result_opencv[i] + << std::endl; ret = EXIT_FAILURE; } } @@ -405,8 +390,8 @@ int main(int argc, const char *argv[]) // Compare results for (unsigned int i = 0; i < bench.size(); i++) { if (std::fabs(result_gsl[i] - result_lapack[i]) > 1e-6) { - std::cout << "Determinant differ between GSL and Lapack: " - << result_gsl[i] << " " << result_lapack[i] << std::endl; + std::cout << "Determinant differ between GSL and Lapack: " << result_gsl[i] << " " << result_lapack[i] + << std::endl; ret = EXIT_FAILURE; } } @@ -415,8 +400,8 @@ int main(int argc, const char *argv[]) // Compare results for (unsigned int i = 0; i < bench.size(); i++) { if (std::fabs(result_gsl[i] - result_eigen3[i]) > 1e-6) { - std::cout << "Determinant differ between GSL and Eigen3: " - << result_gsl[i] << " " << result_eigen3[i] << std::endl; + std::cout << "Determinant differ between GSL and Eigen3: " << result_gsl[i] << " " << result_eigen3[i] + << std::endl; ret = EXIT_FAILURE; } } @@ -425,8 +410,7 @@ int main(int argc, const char *argv[]) // Compare results for (unsigned int i = 0; i < bench.size(); i++) { if (std::fabs(result_lapack[i] - result_opencv[i]) > 1e-6) { - std::cout << "Determinant differ between Lapack and OpenCV: " - << result_lapack[i] << " " << result_opencv[i] + std::cout << "Determinant differ between Lapack and OpenCV: " << result_lapack[i] << " " << result_opencv[i] << std::endl; ret = EXIT_FAILURE; } @@ -436,8 +420,7 @@ int main(int argc, const char *argv[]) // Compare results for (unsigned int i = 0; i < bench.size(); i++) { if (std::fabs(result_eigen3[i] - result_opencv[i]) > 1e-6) { - std::cout << "Determinant differ between Eigen3 and OpenCV: " - << result_eigen3[i] << " " << result_opencv[i] + std::cout << "Determinant differ between Eigen3 and OpenCV: " << result_eigen3[i] << " " << result_opencv[i] << std::endl; ret = EXIT_FAILURE; } @@ -447,8 +430,7 @@ int main(int argc, const char *argv[]) // Compare results for (unsigned int i = 0; i < bench.size(); i++) { if (std::fabs(result_eigen3[i] - result_lapack[i]) > 1e-6) { - std::cout << "Determinant differ between Eigen3 and Lapack: " - << result_eigen3[i] << " " << result_lapack[i] + std::cout << "Determinant differ between Eigen3 and Lapack: " << result_eigen3[i] << " " << result_lapack[i] << std::endl; ret = EXIT_FAILURE; } diff --git a/modules/core/test/math/testMatrixInverse.cpp b/modules/core/test/math/testMatrixInverse.cpp index df90113ec7..58f0132772 100644 --- a/modules/core/test/math/testMatrixInverse.cpp +++ b/modules/core/test/math/testMatrixInverse.cpp @@ -119,10 +119,8 @@ OPTIONS: Default\n\ \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) +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; @@ -205,28 +203,23 @@ vpMatrix make_random_symmetric_positive_matrix(unsigned int n) return A; } -void create_bench_random_matrix(unsigned int nb_matrices, - unsigned int nb_rows, unsigned int nb_cols, - bool verbose, std::vector &bench) +void create_bench_random_matrix(unsigned int nb_matrices, unsigned int nb_rows, unsigned int nb_cols, bool verbose, + std::vector &bench) { if (verbose) - std::cout << "Create a bench of " << nb_matrices << " " << nb_rows - << " by " << nb_cols << " matrices" << std::endl; + std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_cols << " matrices" << std::endl; bench.clear(); for (unsigned int i = 0; i < nb_matrices; i++) { vpMatrix M; -#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || \ - (VISP_HAVE_OPENCV_VERSION >= 0x020101) || defined(VISP_HAVE_GSL) +#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || (VISP_HAVE_OPENCV_VERSION >= 0x020101) || \ + defined(VISP_HAVE_GSL) double det = 0.; // don't put singular matrices in the benchmark - for (M = make_random_matrix(nb_rows, nb_cols); - std::fabs(det = M.AtA().det()) < .01; + for (M = make_random_matrix(nb_rows, nb_cols); std::fabs(det = M.AtA().det()) < .01; M = make_random_matrix(nb_rows, nb_cols)) { if (verbose) { - std::cout << " Generated random matrix AtA=" << std::endl - << M.AtA() << std::endl; - std::cout << " Generated random matrix not invertible: det=" << det - << ". Retrying..." << std::endl; + std::cout << " Generated random matrix AtA=" << std::endl << M.AtA() << std::endl; + std::cout << " Generated random matrix not invertible: det=" << det << ". Retrying..." << std::endl; } } #else @@ -236,27 +229,23 @@ void create_bench_random_matrix(unsigned int nb_matrices, } } -void create_bench_symmetric_positive_matrix(unsigned int nb_matrices, - unsigned int n, bool verbose, +void create_bench_symmetric_positive_matrix(unsigned int nb_matrices, unsigned int n, bool verbose, std::vector &bench) { if (verbose) - std::cout << "Create a bench of " << nb_matrices << " " << n << " by " - << n << " symmetric positive matrices" << std::endl; + std::cout << "Create a bench of " << nb_matrices << " " << n << " by " << n << " symmetric positive matrices" + << std::endl; bench.clear(); for (unsigned int i = 0; i < nb_matrices; i++) { vpMatrix M; -#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || \ - (VISP_HAVE_OPENCV_VERSION >= 0x020101) || defined(VISP_HAVE_GSL) +#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || (VISP_HAVE_OPENCV_VERSION >= 0x020101) || \ + defined(VISP_HAVE_GSL) double det = 0.; // don't put singular matrices in the benchmark - for (M = make_random_symmetric_positive_matrix(n); - std::fabs(det = M.det()) < .01; + for (M = make_random_symmetric_positive_matrix(n); std::fabs(det = M.det()) < .01; M = make_random_symmetric_positive_matrix(n)) { if (verbose) { - std::cout << " Generated random symmetric positive matrix A=" - << std::endl - << M << std::endl; + std::cout << " Generated random symmetric positive matrix A=" << std::endl << M << std::endl; std::cout << " Generated random symmetric positive matrix not " "invertibleL: det=" << det << ". Retrying..." << std::endl; @@ -269,15 +258,13 @@ void create_bench_symmetric_positive_matrix(unsigned int nb_matrices, } } -int test_inverse(const std::vector &bench, - const std::vector &result) +int test_inverse(const std::vector &bench, const std::vector &result) { for (unsigned int i = 0; i < bench.size(); i++) { vpMatrix I = bench[i] * result[i]; - if (std::fabs(I.euclideanNorm() - sqrt(bench[0].AtA().getRows())) > - 1e-10) { - std::cout << "Bad inverse[" << i << "]: " << I.euclideanNorm() << " " - << sqrt(bench[0].AtA().getRows()) << std::endl; + if (std::fabs(I.euclideanNorm() - sqrt(bench[0].AtA().getRows())) > 1e-10) { + std::cout << "Bad inverse[" << i << "]: " << I.euclideanNorm() << " " << sqrt(bench[0].AtA().getRows()) + << std::endl; return EXIT_FAILURE; } } @@ -285,15 +272,13 @@ int test_inverse(const std::vector &bench, } #if defined(VISP_HAVE_EIGEN3) -int test_inverse_lu_eigen3(bool verbose, const std::vector &bench, - double &time) +int test_inverse_lu_eigen3(bool verbose, const std::vector &bench, double &time) { if (verbose) std::cout << "Test inverse by LU using Eigen3 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " Inverting " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() + std::cout << " Inverting " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << " matrix using LU decomposition (Eigen3)." << std::endl; std::vector result(bench.size()); double t = vpTime::measureTimeMs(); @@ -308,15 +293,13 @@ int test_inverse_lu_eigen3(bool verbose, const std::vector &bench, #endif #if defined(VISP_HAVE_LAPACK) -int test_inverse_lu_lapack(bool verbose, const std::vector &bench, - double &time) +int test_inverse_lu_lapack(bool verbose, const std::vector &bench, double &time) { if (verbose) std::cout << "Test inverse by LU using Lapack 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " Inverting " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() + std::cout << " Inverting " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << " matrix using LU decomposition (Lapack)." << std::endl; std::vector result(bench.size()); double t = vpTime::measureTimeMs(); @@ -329,24 +312,18 @@ int test_inverse_lu_lapack(bool verbose, const std::vector &bench, return test_inverse(bench, result); } -int test_inverse_cholesky_lapack(bool verbose, - const std::vector &bench, - double &time) +int test_inverse_cholesky_lapack(bool verbose, const std::vector &bench, double &time) { if (verbose) - std::cout << "Test inverse by Cholesky using Lapack 3rd party" - << std::endl; + std::cout << "Test inverse by Cholesky using Lapack 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " Inverting " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() - << " matrix using cholesky decomposition (Lapack)." - << std::endl; + std::cout << " Inverting " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() + << " matrix using cholesky decomposition (Lapack)." << std::endl; std::vector result(bench.size()); double t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - result[i] = - bench[i].AtA().inverseByCholeskyLapack() * bench[i].transpose(); + result[i] = bench[i].AtA().inverseByCholeskyLapack() * bench[i].transpose(); } time = vpTime::measureTimeMs() - t; @@ -354,15 +331,13 @@ int test_inverse_cholesky_lapack(bool verbose, return test_inverse(bench, result); } -int test_inverse_qr_lapack(bool verbose, const std::vector &bench, - double &time) +int test_inverse_qr_lapack(bool verbose, const std::vector &bench, double &time) { if (verbose) std::cout << "Test inverse by QR using Lapack 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " Inverting " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() + std::cout << " Inverting " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << " matrix using QR decomposition (Lapack)" << std::endl; std::vector result(bench.size()); double t = vpTime::measureTimeMs(); @@ -377,15 +352,13 @@ int test_inverse_qr_lapack(bool verbose, const std::vector &bench, #endif #if defined(VISP_HAVE_GSL) -int test_inverse_lu_gsl(bool verbose, const std::vector &bench, - double &time) +int test_inverse_lu_gsl(bool verbose, const std::vector &bench, double &time) { if (verbose) std::cout << "Test inverse by LU using GSL 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " Inverting " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() + std::cout << " Inverting " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << " matrix using LU decomposition (GSL)" << std::endl; std::vector result(bench.size()); double t = vpTime::measureTimeMs(); @@ -400,15 +373,13 @@ int test_inverse_lu_gsl(bool verbose, const std::vector &bench, #endif #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) -int test_inverse_lu_opencv(bool verbose, const std::vector &bench, - double &time) +int test_inverse_lu_opencv(bool verbose, const std::vector &bench, double &time) { if (verbose) std::cout << "Test inverse by LU using OpenCV 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " Inverting " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() + std::cout << " Inverting " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << " matrix using LU decomposition (OpenCV)" << std::endl; std::vector result(bench.size()); double t = vpTime::measureTimeMs(); @@ -421,23 +392,18 @@ int test_inverse_lu_opencv(bool verbose, const std::vector &bench, return test_inverse(bench, result); } -int test_inverse_cholesky_opencv(bool verbose, - const std::vector &bench, - double &time) +int test_inverse_cholesky_opencv(bool verbose, const std::vector &bench, double &time) { if (verbose) - std::cout << "Test inverse by Cholesky using OpenCV 3rd party" - << std::endl; + std::cout << "Test inverse by Cholesky using OpenCV 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " Inverting " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() + std::cout << " Inverting " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << " matrix using Cholesky decomposition (OpenCV)" << std::endl; std::vector result(bench.size()); double t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - result[i] = - bench[i].AtA().inverseByCholeskyOpenCV() * bench[i].transpose(); + result[i] = bench[i].AtA().inverseByCholeskyOpenCV() * bench[i].transpose(); } time = vpTime::measureTimeMs() - t; @@ -446,11 +412,10 @@ int test_inverse_cholesky_opencv(bool verbose, } #endif -#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || \ - (VISP_HAVE_OPENCV_VERSION >= 0x020101) || defined(VISP_HAVE_GSL) +#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || (VISP_HAVE_OPENCV_VERSION >= 0x020101) || \ + defined(VISP_HAVE_GSL) // SVD is only available for these 3rd parties -int test_pseudo_inverse(bool verbose, const std::vector &bench, - double &time) +int test_pseudo_inverse(bool verbose, const std::vector &bench, double &time) { if (verbose) std::cout << "Test pseudo inverse using either Eigen3, Lapack, OpenCV or " @@ -458,8 +423,8 @@ int test_pseudo_inverse(bool verbose, const std::vector &bench, << std::endl; // Compute inverse if (verbose) - std::cout << " Pseudo inverting " << bench[0].AtA().getRows() << "x" - << bench[0].AtA().getCols() << " matrix" << std::endl; + std::cout << " Pseudo inverting " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << " matrix" + << std::endl; std::vector result(bench.size()); double t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { @@ -472,8 +437,7 @@ int test_pseudo_inverse(bool verbose, const std::vector &bench, } #endif -void save_time(const std::string &method, bool verbose, bool use_plot_file, - std::ofstream &of, double time) +void save_time(const std::string &method, bool verbose, bool use_plot_file, std::ofstream &of, double time) { if (use_plot_file) of << time << "\t"; @@ -485,8 +449,8 @@ void save_time(const std::string &method, bool verbose, bool use_plot_file, int main(int argc, const char *argv[]) { try { -#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || \ - (VISP_HAVE_OPENCV_VERSION >= 0x020101) || defined(VISP_HAVE_GSL) +#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || (VISP_HAVE_OPENCV_VERSION >= 0x020101) || \ + defined(VISP_HAVE_GSL) unsigned int nb_matrices = 1000; unsigned int nb_iterations = 10; unsigned int nb_rows = 6; @@ -497,8 +461,8 @@ int main(int argc, const char *argv[]) std::ofstream of; // Read the command line options - if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, - plotfile, nb_rows, nb_cols, verbose) == false) { + if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, plotfile, nb_rows, nb_cols, verbose) == + false) { exit(-1); } @@ -539,8 +503,7 @@ int main(int argc, const char *argv[]) << "\t"; #endif -#if defined(VISP_HAVE_LAPACK) || (VISP_HAVE_OPENCV_VERSION >= 0x020101) || \ - defined(VISP_HAVE_GSL) +#if defined(VISP_HAVE_LAPACK) || (VISP_HAVE_OPENCV_VERSION >= 0x020101) || defined(VISP_HAVE_GSL) of << "\"Pseudo inverse (Lapack, OpenCV, GSL)\"" << "\t"; #endif @@ -550,11 +513,9 @@ int main(int argc, const char *argv[]) int ret = EXIT_SUCCESS; for (unsigned int iter = 0; iter < nb_iterations; iter++) { std::vector bench_random_matrices; - create_bench_random_matrix(nb_matrices, nb_rows, nb_cols, verbose, - bench_random_matrices); + create_bench_random_matrix(nb_matrices, nb_rows, nb_cols, verbose, bench_random_matrices); std::vector bench_symmetric_positive_matrices; - create_bench_symmetric_positive_matrix( - nb_matrices, nb_rows, verbose, bench_symmetric_positive_matrices); + create_bench_symmetric_positive_matrix(nb_matrices, nb_rows, verbose, bench_symmetric_positive_matrices); if (use_plot_file) of << iter << "\t"; @@ -583,17 +544,13 @@ int main(int argc, const char *argv[]) // Cholesky for symmetric positive matrices #if defined(VISP_HAVE_LAPACK) - ret += test_inverse_cholesky_lapack( - verbose, bench_symmetric_positive_matrices, time); - save_time("Inverse by Cholesly (Lapack): ", verbose, use_plot_file, of, - time); + ret += test_inverse_cholesky_lapack(verbose, bench_symmetric_positive_matrices, time); + save_time("Inverse by Cholesly (Lapack): ", verbose, use_plot_file, of, time); #endif #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) - ret += test_inverse_cholesky_opencv( - verbose, bench_symmetric_positive_matrices, time); - save_time("Inverse by Cholesky (OpenCV): ", verbose, use_plot_file, of, - time); + ret += test_inverse_cholesky_opencv(verbose, bench_symmetric_positive_matrices, time); + save_time("Inverse by Cholesky (OpenCV): ", verbose, use_plot_file, of, time); #endif // QR decomposition @@ -603,11 +560,10 @@ int main(int argc, const char *argv[]) #endif // Pseudo-inverse with SVD -#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || \ - (VISP_HAVE_OPENCV_VERSION >= 0x020101) || defined(VISP_HAVE_GSL) +#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || (VISP_HAVE_OPENCV_VERSION >= 0x020101) || \ + defined(VISP_HAVE_GSL) ret += test_pseudo_inverse(verbose, bench_random_matrices, time); - save_time("Pseudo inverse (Lapack, Eigen3, OpenCV or GSL): ", verbose, - use_plot_file, of, time); + save_time("Pseudo inverse (Lapack, Eigen3, OpenCV or GSL): ", verbose, use_plot_file, of, time); #endif if (use_plot_file) diff --git a/modules/core/test/math/testMatrixPseudoInverse.cpp b/modules/core/test/math/testMatrixPseudoInverse.cpp index d48f7bb1b5..c21b62f5d0 100644 --- a/modules/core/test/math/testMatrixPseudoInverse.cpp +++ b/modules/core/test/math/testMatrixPseudoInverse.cpp @@ -116,10 +116,8 @@ OPTIONS: Default\n\ \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) +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; @@ -189,13 +187,11 @@ vpMatrix make_random_matrix(unsigned int nbrows, unsigned int nbcols) return A; } -void create_bench_random_matrix(unsigned int nb_matrices, - unsigned int nb_rows, unsigned int nb_cols, - bool verbose, std::vector &bench) +void create_bench_random_matrix(unsigned int nb_matrices, unsigned int nb_rows, unsigned int nb_cols, bool verbose, + std::vector &bench) { if (verbose) - std::cout << "Create a bench of " << nb_matrices << " " << nb_rows - << " by " << nb_cols << " matrices" << std::endl; + std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_cols << " matrices" << std::endl; bench.clear(); for (unsigned int i = 0; i < nb_matrices; i++) { vpMatrix M = make_random_matrix(nb_rows, nb_cols); @@ -203,36 +199,30 @@ void create_bench_random_matrix(unsigned int nb_matrices, } } -int test_pseudo_inverse(const std::vector &A, - const std::vector &Api) +int test_pseudo_inverse(const std::vector &A, const std::vector &Api) { double allowed_error = 1e-3; for (unsigned int i = 0; i < A.size(); i++) { double error = (A[i] * Api[i] * A[i] - A[i]).euclideanNorm(); if (error > allowed_error) { - std::cout << "Bad pseudo-inverse [" << i - << "]: euclidean norm: " << error << std::endl; + std::cout << "Bad pseudo-inverse [" << i << "]: euclidean norm: " << error << std::endl; return EXIT_FAILURE; } } return EXIT_SUCCESS; } -int test_pseudo_inverse(const std::vector &A, - const std::vector &Api, - const std::vector &sv, - const std::vector &imA, - const std::vector &imAt, - const std::vector &kerAt) +int test_pseudo_inverse(const std::vector &A, const std::vector &Api, + const std::vector &sv, const std::vector &imA, + const std::vector &imAt, const std::vector &kerAt) { double allowed_error = 1e-3; // test Api for (unsigned int i = 0; i < A.size(); i++) { double error = (A[i] * Api[i] * A[i] - A[i]).euclideanNorm(); if (error > allowed_error) { - std::cout << "Bad pseudo-inverse [" << i - << "]: euclidean norm: " << error << std::endl; + std::cout << "Bad pseudo-inverse [" << i << "]: euclidean norm: " << error << std::endl; return EXIT_FAILURE; } } @@ -244,8 +234,7 @@ int test_pseudo_inverse(const std::vector &A, double error = nullspace.euclideanNorm(); if (error > allowed_error) { - std::cout << "Bad kernel [" << i << "]: euclidean norm: " << error - << std::endl; + std::cout << "Bad kernel [" << i << "]: euclidean norm: " << error << std::endl; return EXIT_FAILURE; } } @@ -266,8 +255,7 @@ int test_pseudo_inverse(const std::vector &A, double error = (U * S * Vt - A[i]).euclideanNorm(); if (error > allowed_error) { - std::cout << "Bad imA, imAt, sv, kerAt [" << i - << "]: euclidean norm: " << error << std::endl; + std::cout << "Bad imA, imAt, sv, kerAt [" << i << "]: euclidean norm: " << error << std::endl; return EXIT_FAILURE; } } @@ -275,15 +263,12 @@ int test_pseudo_inverse(const std::vector &A, return EXIT_SUCCESS; } -int test_pseudo_inverse_default(bool verbose, - const std::vector &bench, - std::vector &time) +int test_pseudo_inverse_default(bool verbose, const std::vector &bench, std::vector &time) { if (verbose) std::cout << "Test pseudo-inverse using default 3rd party" << std::endl; if (verbose) - std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" - << bench[0].getCols() << " matrix" << std::endl; + std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl; size_t size = bench.size(); std::vector PI(size), imA(size), imAt(size), kerAt(size); @@ -339,15 +324,12 @@ int test_pseudo_inverse_default(bool verbose, } #if defined(VISP_HAVE_EIGEN3) -int test_pseudo_inverse_eigen3(bool verbose, - const std::vector &bench, - std::vector &time) +int test_pseudo_inverse_eigen3(bool verbose, const std::vector &bench, std::vector &time) { if (verbose) std::cout << "Test pseudo-inverse using Eigen3 3rd party" << std::endl; if (verbose) - std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" - << bench[0].getCols() << " matrix" << std::endl; + std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl; size_t size = bench.size(); std::vector PI(size), imA(size), imAt(size), kerAt(size); @@ -391,8 +373,7 @@ int test_pseudo_inverse_eigen3(bool verbose, test++; t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverseEigen3(PI[i], sv[i], 1e-6, imA[i], imAt[i], - kerAt[i]); + bench[i].pseudoInverseEigen3(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]); } time[test] = vpTime::measureTimeMs() - t; @@ -405,15 +386,12 @@ int test_pseudo_inverse_eigen3(bool verbose, #endif #if defined(VISP_HAVE_LAPACK) -int test_pseudo_inverse_lapack(bool verbose, - const std::vector &bench, - std::vector &time) +int test_pseudo_inverse_lapack(bool verbose, const std::vector &bench, std::vector &time) { if (verbose) std::cout << "Test pseudo-inverse using Lapack 3rd party" << std::endl; if (verbose) - std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" - << bench[0].getCols() << " matrix" << std::endl; + std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl; size_t size = bench.size(); std::vector PI(size), imA(size), imAt(size), kerAt(size); @@ -457,8 +435,7 @@ int test_pseudo_inverse_lapack(bool verbose, test++; t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverseLapack(PI[i], sv[i], 1e-6, imA[i], imAt[i], - kerAt[i]); + bench[i].pseudoInverseLapack(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]); } time[test] = vpTime::measureTimeMs() - t; @@ -471,14 +448,12 @@ int test_pseudo_inverse_lapack(bool verbose, #endif #if defined(VISP_HAVE_GSL) -int test_pseudo_inverse_gsl(bool verbose, const std::vector &bench, - std::vector &time) +int test_pseudo_inverse_gsl(bool verbose, const std::vector &bench, std::vector &time) { if (verbose) std::cout << "Test pseudo-inverse using Gsl 3rd party" << std::endl; if (verbose) - std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" - << bench[0].getCols() << " matrix" << std::endl; + std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl; size_t size = bench.size(); std::vector PI(size), imA(size), imAt(size), kerAt(size); @@ -535,15 +510,12 @@ int test_pseudo_inverse_gsl(bool verbose, const std::vector &bench, #endif #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) -int test_pseudo_inverse_opencv(bool verbose, - const std::vector &bench, - std::vector &time) +int test_pseudo_inverse_opencv(bool verbose, const std::vector &bench, std::vector &time) { if (verbose) std::cout << "Test pseudo-inverse using OpenCV 3rd party" << std::endl; if (verbose) - std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" - << bench[0].getCols() << " matrix" << std::endl; + std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl; size_t size = bench.size(); std::vector PI(size), imA(size), imAt(size), kerAt(size); @@ -587,8 +559,7 @@ int test_pseudo_inverse_opencv(bool verbose, test++; t = vpTime::measureTimeMs(); for (unsigned int i = 0; i < bench.size(); i++) { - bench[i].pseudoInverseOpenCV(PI[i], sv[i], 1e-6, imA[i], imAt[i], - kerAt[i]); + bench[i].pseudoInverseOpenCV(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]); } time[test] = vpTime::measureTimeMs() - t; @@ -600,8 +571,7 @@ int test_pseudo_inverse_opencv(bool verbose, } #endif -void save_time(const std::string &method, unsigned int nrows, - unsigned int ncols, bool verbose, bool use_plot_file, +void save_time(const std::string &method, unsigned int nrows, unsigned int ncols, bool verbose, bool use_plot_file, std::ofstream &of, const std::vector &time) { for (size_t i = 0; i < time.size(); i++) { @@ -617,8 +587,8 @@ void save_time(const std::string &method, unsigned int nrows, int main(int argc, const char *argv[]) { try { -#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || \ - (VISP_HAVE_OPENCV_VERSION >= 0x020101) || defined(VISP_HAVE_GSL) +#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || (VISP_HAVE_OPENCV_VERSION >= 0x020101) || \ + defined(VISP_HAVE_GSL) unsigned int nb_matrices = 500; unsigned int nb_iterations = 10; unsigned int nb_rows = 12; @@ -628,16 +598,14 @@ int main(int argc, const char *argv[]) bool use_plot_file = false; std::ofstream of; - unsigned int nb_svd_functions = - 4; // 4 tests for each existing vpMatrix::pseudoInverse(...) functions + unsigned int nb_svd_functions = 4; // 4 tests for each existing vpMatrix::pseudoInverse(...) functions unsigned int nb_test_matrix_size = 3; // 3 tests: m > n, m = n, m < n std::vector time(nb_svd_functions); - std::vector nrows(nb_test_matrix_size), - ncols(nb_test_matrix_size); + std::vector nrows(nb_test_matrix_size), ncols(nb_test_matrix_size); // Read the command line options - if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, - plotfile, nb_rows, nb_cols, verbose) == false) { + if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, plotfile, nb_rows, nb_cols, verbose) == + false) { exit(-1); } @@ -662,32 +630,27 @@ int main(int argc, const char *argv[]) for (unsigned int s = 0; s < nb_test_matrix_size; s++) { for (unsigned int i = 0; i < nb_svd_functions; i++) - of << "\"default " << nrows[s] << "x" << ncols[s] << " test " << i - << "\"" + of << "\"default " << nrows[s] << "x" << ncols[s] << " test " << i << "\"" << "\t"; #if defined(VISP_HAVE_LAPACK) for (unsigned int i = 0; i < nb_svd_functions; i++) - of << "\"Lapack " << nrows[s] << "x" << ncols[s] << " test " << i - << "\"" + of << "\"Lapack " << nrows[s] << "x" << ncols[s] << " test " << i << "\"" << "\t"; #endif #if defined(VISP_HAVE_EIGEN3) for (unsigned int i = 0; i < nb_svd_functions; i++) - of << "\"Eigen3 " << nrows[s] << "x" << ncols[s] << " test " << i - << "\"" + of << "\"Eigen3 " << nrows[s] << "x" << ncols[s] << " test " << i << "\"" << "\t"; #endif #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) for (unsigned int i = 0; i < nb_svd_functions; i++) - of << "\"OpenCV " << nrows[s] << "x" << ncols[s] << " test " << i - << "\"" + of << "\"OpenCV " << nrows[s] << "x" << ncols[s] << " test " << i << "\"" << "\t"; #endif #if defined(VISP_HAVE_GSL) for (unsigned int i = 0; i < nb_svd_functions; i++) - of << "\"GSL " << nrows[s] << "x" << ncols[s] << " test " << i - << "\"" + of << "\"GSL " << nrows[s] << "x" << ncols[s] << " test " << i << "\"" << "\t"; #endif } @@ -702,39 +665,29 @@ int main(int argc, const char *argv[]) for (unsigned int s = 0; s < nb_test_matrix_size; s++) { std::vector bench_random_matrices; - create_bench_random_matrix(nb_matrices, nrows[s], ncols[s], verbose, - bench_random_matrices); + create_bench_random_matrix(nb_matrices, nrows[s], ncols[s], verbose, bench_random_matrices); - ret += - test_pseudo_inverse_default(verbose, bench_random_matrices, time); - save_time("default -", nrows[s], ncols[s], verbose, use_plot_file, of, - time); + ret += test_pseudo_inverse_default(verbose, bench_random_matrices, time); + save_time("default -", nrows[s], ncols[s], verbose, use_plot_file, of, time); #if defined(VISP_HAVE_LAPACK) - ret += - test_pseudo_inverse_lapack(verbose, bench_random_matrices, time); - save_time("Lapack -", nrows[s], ncols[s], verbose, use_plot_file, of, - time); + ret += test_pseudo_inverse_lapack(verbose, bench_random_matrices, time); + save_time("Lapack -", nrows[s], ncols[s], verbose, use_plot_file, of, time); #endif #if defined(VISP_HAVE_EIGEN3) - ret += - test_pseudo_inverse_eigen3(verbose, bench_random_matrices, time); - save_time("Eigen3 -", nrows[s], ncols[s], verbose, use_plot_file, of, - time); + ret += test_pseudo_inverse_eigen3(verbose, bench_random_matrices, time); + save_time("Eigen3 -", nrows[s], ncols[s], verbose, use_plot_file, of, time); #endif #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) - ret += - test_pseudo_inverse_opencv(verbose, bench_random_matrices, time); - save_time("OpenCV -", nrows[s], ncols[s], verbose, use_plot_file, of, - time); + ret += test_pseudo_inverse_opencv(verbose, bench_random_matrices, time); + save_time("OpenCV -", nrows[s], ncols[s], verbose, use_plot_file, of, time); #endif #if defined(VISP_HAVE_GSL) ret += test_pseudo_inverse_gsl(verbose, bench_random_matrices, time); - save_time("GSL -", nrows[s], ncols[s], verbose, use_plot_file, of, - time); + save_time("GSL -", nrows[s], ncols[s], verbose, use_plot_file, of, time); #endif } if (use_plot_file) diff --git a/modules/core/test/math/testPoseVector.cpp b/modules/core/test/math/testPoseVector.cpp index ae36f31d21..49d9aa993d 100644 --- a/modules/core/test/math/testPoseVector.cpp +++ b/modules/core/test/math/testPoseVector.cpp @@ -48,20 +48,17 @@ #include -bool test(const std::string &s, const vpArray2D &A, - const std::vector &bench) +bool test(const std::string &s, const vpArray2D &A, const std::vector &bench) { static unsigned int cpt = 0; std::cout << "** Test " << ++cpt << std::endl; - std::cout << s << "(" << A.getRows() << "," << A.getCols() << ") =" << A - << 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::epsilon()) { + if (std::fabs(A.data[i] - bench[i]) > std::fabs(A.data[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return false; } diff --git a/modules/core/test/math/testQuaternion.cpp b/modules/core/test/math/testQuaternion.cpp index 2bddcd1e57..5d3a53f5cb 100644 --- a/modules/core/test/math/testQuaternion.cpp +++ b/modules/core/test/math/testQuaternion.cpp @@ -56,26 +56,19 @@ int main() std::cout << "q3=" << q3 << std::endl; if (!vpMath::equal(q3.x(), 2.6, std::numeric_limits::epsilon()) || !vpMath::equal(q3.y(), 0.4, std::numeric_limits::epsilon()) || - !vpMath::equal(q3.z(), -3.0, - std::numeric_limits::epsilon()) || + !vpMath::equal(q3.z(), -3.0, std::numeric_limits::epsilon()) || !vpMath::equal(q3.w(), 4.0, std::numeric_limits::epsilon())) { - throw vpException(vpException::fatalError, - "Problem with addition of two quaternions !"); + 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::epsilon() * 1e4) || - !vpMath::equal(q4.y(), q2.y(), - std::numeric_limits::epsilon() * 1e4) || - !vpMath::equal(q4.z(), q2.z(), - std::numeric_limits::epsilon() * 1e4) || - !vpMath::equal(q4.w(), q2.w(), - std::numeric_limits::epsilon() * 1e4)) { - throw vpException(vpException::fatalError, - "Problem with subtraction of two quaternions !"); + if (!vpMath::equal(q4.x(), q2.x(), std::numeric_limits::epsilon() * 1e4) || + !vpMath::equal(q4.y(), q2.y(), std::numeric_limits::epsilon() * 1e4) || + !vpMath::equal(q4.z(), q2.z(), std::numeric_limits::epsilon() * 1e4) || + !vpMath::equal(q4.w(), q2.w(), std::numeric_limits::epsilon() * 1e4)) { + throw vpException(vpException::fatalError, "Problem with subtraction of two quaternions !"); } // Test multiplication of two quaternions @@ -84,61 +77,44 @@ int main() 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::epsilon() * 1e4) || - !vpMath::equal(q7.y(), 36.7, - std::numeric_limits::epsilon() * 1e4) || - !vpMath::equal(q7.z(), -6.6, - std::numeric_limits::epsilon() * 1e4) || - !vpMath::equal(q7.w(), 1.3, - std::numeric_limits::epsilon() * 1e4)) { - throw vpException(vpException::fatalError, - "Problem with multiplication of two quaternions !"); + if (!vpMath::equal(q7.x(), 3.0, std::numeric_limits::epsilon() * 1e4) || + !vpMath::equal(q7.y(), 36.7, std::numeric_limits::epsilon() * 1e4) || + !vpMath::equal(q7.z(), -6.6, std::numeric_limits::epsilon() * 1e4) || + !vpMath::equal(q7.w(), 1.3, std::numeric_limits::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::epsilon() * 1e4) || - !vpMath::equal(q7_conj.y(), -36.7, - std::numeric_limits::epsilon() * 1e4) || - !vpMath::equal(q7_conj.z(), 6.6, - std::numeric_limits::epsilon() * 1e4) || - !vpMath::equal(q7_conj.w(), 1.3, - std::numeric_limits::epsilon() * 1e4)) { - throw vpException(vpException::fatalError, - "Problem with quaternion conjugate !"); + if (!vpMath::equal(q7_conj.x(), -3.0, std::numeric_limits::epsilon() * 1e4) || + !vpMath::equal(q7_conj.y(), -36.7, std::numeric_limits::epsilon() * 1e4) || + !vpMath::equal(q7_conj.z(), 6.6, std::numeric_limits::epsilon() * 1e4) || + !vpMath::equal(q7_conj.w(), 1.3, std::numeric_limits::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 !"); + 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 !"); + 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 !"); + 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 diff --git a/modules/core/test/math/testRobust.cpp b/modules/core/test/math/testRobust.cpp index 27bc70825b..042eb4450b 100644 --- a/modules/core/test/math/testRobust.cpp +++ b/modules/core/test/math/testRobust.cpp @@ -167,8 +167,7 @@ int main(int argc, const char **argv) 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; + std::cerr << " Check your -o " << ofilename << " option " << std::endl; exit(-1); } } diff --git a/modules/core/test/math/testRotation.cpp b/modules/core/test/math/testRotation.cpp index 08f57cccdf..c24d76add2 100644 --- a/modules/core/test/math/testRotation.cpp +++ b/modules/core/test/math/testRotation.cpp @@ -54,19 +54,16 @@ static unsigned int cpt = 0; -bool test(const std::string &s, const vpArray2D &v, - const std::vector &bench) +bool test(const std::string &s, const vpArray2D &v, const std::vector &bench) { std::cout << "** Test " << ++cpt << std::endl; - std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v - << "]" << 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::epsilon()) { + if (std::fabs(v.data[i] - bench[i]) > std::fabs(v.data[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return false; } @@ -75,15 +72,12 @@ bool test(const std::string &s, const vpArray2D &v, return true; } -bool test(const std::string &s, const vpRotationVector &v, - const double &bench) +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; + 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::epsilon()) { + if (std::fabs(v[i] - bench) > std::fabs(v[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return false; } @@ -115,8 +109,7 @@ int main() 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::epsilon()) { + if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return EXIT_FAILURE; } @@ -151,8 +144,7 @@ int main() 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::epsilon()) { + if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return EXIT_FAILURE; } @@ -187,8 +179,7 @@ int main() 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::epsilon()) { + if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return EXIT_FAILURE; } @@ -223,8 +214,7 @@ int main() 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::epsilon()) { + if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return EXIT_FAILURE; } @@ -240,8 +230,7 @@ int main() return EXIT_FAILURE; } { - vpQuaternionVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), - vpMath::rad(10)); + vpQuaternionVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); std::vector bench1(4, vpMath::rad(10)); if (test("r1", r1, bench1) == false) return EXIT_FAILURE; @@ -254,15 +243,13 @@ int main() return EXIT_FAILURE; vpQuaternionVector r3; - r3.set(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), - vpMath::rad(10)); + r3.set(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); if (test("r3", r3, bench1) == false) return EXIT_FAILURE; 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::epsilon()) { + if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return EXIT_FAILURE; } @@ -281,10 +268,8 @@ int main() 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 + 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; @@ -295,8 +280,7 @@ int main() std::cout << "theta=" << vpMath::deg(theta) << std::endl; std::cout << "u=" << u << std::endl; - std::cout << "From vpThetaUVector to vpRotationMatrix " - << std::endl; + std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl; R.buildFrom(tu); std::cout << "Matrix R"; @@ -307,31 +291,26 @@ int main() std::cout << R << std::endl; - std::cout << "From vpRotationMatrix to vpQuaternionVector " - << 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 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; + 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; + std::cout << "result : should equivalent to the first one " << std::endl; double theta2; vpColVector u2; @@ -340,17 +319,12 @@ int main() std::cout << "theta=" << vpMath::deg(theta2) << std::endl; std::cout << "u=" << u2 << std::endl; - assert(vpMath::abs(theta2 - theta) < - std::numeric_limits::epsilon() * 1e10); - assert(vpMath::abs(u[0] - u2[0]) < - std::numeric_limits::epsilon() * 1e10); - assert(vpMath::abs(u[1] - u2[1]) < - std::numeric_limits::epsilon() * 1e10); - assert(vpMath::abs(u[2] - u2[2]) < - std::numeric_limits::epsilon() * 1e10); + assert(vpMath::abs(theta2 - theta) < std::numeric_limits::epsilon() * 1e10); + assert(vpMath::abs(u[0] - u2[0]) < std::numeric_limits::epsilon() * 1e10); + assert(vpMath::abs(u[1] - u2[1]) < std::numeric_limits::epsilon() * 1e10); + assert(vpMath::abs(u[2] - u2[2]) < std::numeric_limits::epsilon() * 1e10); } - vpRzyzVector rzyz(vpMath::rad(180), vpMath::rad(120), - vpMath::rad(45)); + 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; @@ -360,8 +334,7 @@ int main() rzyz_final.buildFrom(R); std::cout << rzyz_final << std::endl; - vpRzyxVector rzyx(vpMath::rad(180), vpMath::rad(120), - vpMath::rad(45)); + 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; diff --git a/modules/core/test/math/testRowVector.cpp b/modules/core/test/math/testRowVector.cpp index 27e8547548..04db35ad38 100644 --- a/modules/core/test/math/testRowVector.cpp +++ b/modules/core/test/math/testRowVector.cpp @@ -48,20 +48,17 @@ #include #include -bool test(const std::string &s, const vpRowVector &v, - const std::vector &bench) +bool test(const std::string &s, const vpRowVector &v, const std::vector &bench) { static unsigned int cpt = 0; std::cout << "** Test " << ++cpt << std::endl; - std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v - << "]" << 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::epsilon()) { + if (std::fabs(v[i] - bench[i]) > std::fabs(v[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return false; } diff --git a/modules/core/test/math/testSvd.cpp b/modules/core/test/math/testSvd.cpp index c44b95546e..2889b84d11 100644 --- a/modules/core/test/math/testSvd.cpp +++ b/modules/core/test/math/testSvd.cpp @@ -118,10 +118,8 @@ OPTIONS: Default\n\ \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) +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; @@ -191,13 +189,11 @@ vpMatrix make_random_matrix(unsigned int nbrows, unsigned int nbcols) return A; } -void create_bench_random_matrix(unsigned int nb_matrices, - unsigned int nb_rows, unsigned int nb_cols, - bool verbose, std::vector &bench) +void create_bench_random_matrix(unsigned int nb_matrices, unsigned int nb_rows, unsigned int nb_cols, bool verbose, + std::vector &bench) { if (verbose) - std::cout << "Create a bench of " << nb_matrices << " " << nb_rows - << " by " << nb_cols << " matrices" << std::endl; + std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_cols << " matrices" << std::endl; bench.clear(); for (unsigned int i = 0; i < nb_matrices; i++) { vpMatrix M; @@ -221,8 +217,7 @@ void create_bench_random_matrix(unsigned int nb_matrices, } } -int test_svd(std::vector M, std::vector U, - std::vector s, std::vector V) +int test_svd(std::vector M, std::vector U, std::vector s, std::vector V) { for (unsigned int i = 0; i < M.size(); i++) { vpMatrix S; @@ -238,15 +233,13 @@ int test_svd(std::vector M, std::vector U, } #if defined(VISP_HAVE_EIGEN3) -int test_svd_eigen3(bool verbose, const std::vector &bench, - double &time) +int test_svd_eigen3(bool verbose, const std::vector &bench, double &time) { if (verbose) std::cout << "Test SVD using Eigen3 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " SVD on a " << bench[0].getRows() << "x" - << bench[0].getCols() << " matrix" << std::endl; + std::cout << " SVD on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl; std::vector U = bench; std::vector V(bench.size()); @@ -264,15 +257,13 @@ int test_svd_eigen3(bool verbose, const std::vector &bench, #endif #if defined(VISP_HAVE_LAPACK) -int test_svd_lapack(bool verbose, const std::vector &bench, - double &time) +int test_svd_lapack(bool verbose, const std::vector &bench, double &time) { if (verbose) std::cout << "Test SVD using Lapack 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " SVD on a " << bench[0].getRows() << "x" - << bench[0].getCols() << " matrix" << std::endl; + std::cout << " SVD on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl; std::vector U = bench; std::vector V(bench.size()); @@ -289,15 +280,13 @@ int test_svd_lapack(bool verbose, const std::vector &bench, #endif #if defined(VISP_HAVE_GSL) -int test_svd_gsl(bool verbose, const std::vector &bench, - double &time) +int test_svd_gsl(bool verbose, const std::vector &bench, double &time) { if (verbose) std::cout << "Test SVD using GSL 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " SVD on a " << bench[0].getRows() << "x" - << bench[0].getCols() << " matrix" << std::endl; + std::cout << " SVD on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl; std::vector U = bench; std::vector V(bench.size()); @@ -315,15 +304,13 @@ int test_svd_gsl(bool verbose, const std::vector &bench, #endif #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) -int test_svd_opencv(bool verbose, const std::vector &bench, - double &time) +int test_svd_opencv(bool verbose, const std::vector &bench, double &time) { if (verbose) std::cout << "Test SVD using OpenCV 3rd party" << std::endl; // Compute inverse if (verbose) - std::cout << " SVD on a " << bench[0].getRows() << "x" - << bench[0].getCols() << " matrix" << std::endl; + std::cout << " SVD on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl; std::vector U = bench; std::vector V(bench.size()); @@ -339,8 +326,7 @@ int test_svd_opencv(bool verbose, const std::vector &bench, } #endif -void save_time(const std::string &method, bool verbose, bool use_plot_file, - std::ofstream &of, double time) +void save_time(const std::string &method, bool verbose, bool use_plot_file, std::ofstream &of, double time) { if (use_plot_file) of << time << "\t"; @@ -352,8 +338,8 @@ void save_time(const std::string &method, bool verbose, bool use_plot_file, int main(int argc, const char *argv[]) { try { -#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || \ - (VISP_HAVE_OPENCV_VERSION >= 0x020101) || defined(VISP_HAVE_GSL) +#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || (VISP_HAVE_OPENCV_VERSION >= 0x020101) || \ + defined(VISP_HAVE_GSL) unsigned int nb_matrices = 1000; unsigned int nb_iterations = 10; unsigned int nb_rows = 6; @@ -364,8 +350,8 @@ int main(int argc, const char *argv[]) std::ofstream of; // Read the command line options - if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, - plotfile, nb_rows, nb_cols, verbose) == false) { + if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, plotfile, nb_rows, nb_cols, verbose) == + false) { exit(-1); } @@ -396,8 +382,7 @@ int main(int argc, const char *argv[]) int ret = EXIT_SUCCESS; for (unsigned int iter = 0; iter < nb_iterations; iter++) { std::vector bench_random_matrices; - create_bench_random_matrix(nb_matrices, nb_rows, nb_cols, verbose, - bench_random_matrices); + create_bench_random_matrix(nb_matrices, nb_rows, nb_cols, verbose, bench_random_matrices); if (use_plot_file) of << iter << "\t"; diff --git a/modules/core/test/math/testTranslationVector.cpp b/modules/core/test/math/testTranslationVector.cpp index 88b571c61d..2aa27cc899 100644 --- a/modules/core/test/math/testTranslationVector.cpp +++ b/modules/core/test/math/testTranslationVector.cpp @@ -48,20 +48,17 @@ #include -bool test(const std::string &s, const vpArray2D &A, - const std::vector &bench) +bool test(const std::string &s, const vpArray2D &A, const std::vector &bench) { static unsigned int cpt = 0; std::cout << "** Test " << ++cpt << std::endl; - std::cout << s << "(" << A.getRows() << "," << A.getCols() << ") =" << A - << 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::epsilon()) { + if (std::fabs(A.data[i] - bench[i]) > std::fabs(A.data[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return false; } diff --git a/modules/core/test/network/testClient.cpp b/modules/core/test/network/testClient.cpp index 2cefbfd394..0d5530b046 100644 --- a/modules/core/test/network/testClient.cpp +++ b/modules/core/test/network/testClient.cpp @@ -58,14 +58,12 @@ int main() int val = 0; while (1) { - if (client.send(&val) != - sizeof(int)) // Sending the new value to the first client + 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 + 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; diff --git a/modules/core/test/network/testServer.cpp b/modules/core/test/network/testServer.cpp index caee69b4ed..d088fdb5b1 100644 --- a/modules/core/test/network/testServer.cpp +++ b/modules/core/test/network/testServer.cpp @@ -59,15 +59,13 @@ int main() serv.checkForConnections(); if (serv.getNumberOfClients() > 0) { - if (serv.receive(&val) != - sizeof(int)) // Receiving a value from the first client + 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 + 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; diff --git a/modules/core/test/network/testUDPClient.cpp b/modules/core/test/network/testUDPClient.cpp index 6d76841a16..eeb623729c 100644 --- a/modules/core/test/network/testUDPClient.cpp +++ b/modules/core/test/network/testUDPClient.cpp @@ -66,21 +66,16 @@ int main() DataType data_type(1234.56789, 123450); char data[sizeof(data_type.double_val) + sizeof(data_type.int_val)]; memcpy(data, &data_type.double_val, sizeof(data_type.double_val)); - memcpy(data + sizeof(data_type.double_val), &data_type.int_val, - sizeof(data_type.int_val)); - std::string msg(data, - sizeof(data_type.double_val) + sizeof(data_type.int_val)); - if (client.send(msg) != - (int)sizeof(data_type.double_val) + sizeof(data_type.int_val)) + memcpy(data + sizeof(data_type.double_val), &data_type.int_val, sizeof(data_type.int_val)); + std::string msg(data, sizeof(data_type.double_val) + sizeof(data_type.int_val)); + if (client.send(msg) != (int)sizeof(data_type.double_val) + sizeof(data_type.int_val)) std::cerr << "Error client.send()!" << std::endl; if (client.receive(msg)) { data_type.double_val = *reinterpret_cast(msg.c_str()); - data_type.int_val = *reinterpret_cast( - msg.c_str() + sizeof(data_type.double_val)); + data_type.int_val = *reinterpret_cast(msg.c_str() + sizeof(data_type.double_val)); - std::cout << "Receive from the server double_val: " - << data_type.double_val << " ; int_val: " << data_type.int_val + std::cout << "Receive from the server double_val: " << data_type.double_val << " ; int_val: " << data_type.int_val << std::endl; } diff --git a/modules/core/test/network/testUDPServer.cpp b/modules/core/test/network/testUDPServer.cpp index 4b697fe56e..eaccefb101 100644 --- a/modules/core/test/network/testUDPServer.cpp +++ b/modules/core/test/network/testUDPServer.cpp @@ -69,28 +69,22 @@ int main() int res = server.receive(msg, hostInfo); if (res) { DataType data_type; - memcpy(&data_type.double_val, msg.c_str(), - sizeof(data_type.double_val)); - memcpy(&data_type.int_val, msg.c_str() + sizeof(data_type.double_val), - sizeof(data_type.int_val)); - std::cout << "Server received double_val: " << data_type.double_val - << " ; int_val: " << data_type.int_val + memcpy(&data_type.double_val, msg.c_str(), sizeof(data_type.double_val)); + memcpy(&data_type.int_val, msg.c_str() + sizeof(data_type.double_val), sizeof(data_type.int_val)); + std::cout << "Server received double_val: " << data_type.double_val << " ; int_val: " << data_type.int_val << " from: " << hostInfo << std::endl; // Get address and port std::istringstream iss(hostInfo); std::vector tokens; - std::copy(std::istream_iterator(iss), - std::istream_iterator(), + std::copy(std::istream_iterator(iss), std::istream_iterator(), std::back_inserter(tokens)); data_type.double_val += 1.5; data_type.int_val += 2; char data[sizeof(data_type.double_val) + sizeof(data_type.int_val)]; memcpy(data, &data_type.double_val, sizeof(data_type.double_val)); - memcpy(data + sizeof(data_type.double_val), &data_type.int_val, - sizeof(data_type.int_val)); - msg = std::string(data, sizeof(data_type.double_val) + - sizeof(data_type.int_val)); + memcpy(data + sizeof(data_type.double_val), &data_type.int_val, sizeof(data_type.int_val)); + msg = std::string(data, sizeof(data_type.double_val) + sizeof(data_type.int_val)); server.send(msg, tokens[1], atoi(tokens[2].c_str())); } @@ -99,15 +93,13 @@ int main() while (true) { int res = server.receive(msg, hostInfo, 5000); if (res) { - std::cout << "Server received: " << msg << " from: " << hostInfo - << std::endl; + std::cout << "Server received: " << msg << " from: " << hostInfo << std::endl; std::cout << "Reply to the client: Echo: " << msg << std::endl; // Get address and port std::istringstream iss(hostInfo); std::vector tokens; - std::copy(std::istream_iterator(iss), - std::istream_iterator(), + std::copy(std::istream_iterator(iss), std::istream_iterator(), std::back_inserter(tokens)); server.send("Echo: " + msg, tokens[1], atoi(tokens[2].c_str())); } else if (res == 0) { diff --git a/modules/core/test/tools/convert/testConvert.cpp b/modules/core/test/tools/convert/testConvert.cpp index caca14bd42..da9556ae9f 100644 --- a/modules/core/test/tools/convert/testConvert.cpp +++ b/modules/core/test/tools/convert/testConvert.cpp @@ -49,10 +49,7 @@ #include #include -bool areSame(double a, double b) -{ - return fabs(a - b) < std::numeric_limits::epsilon(); -} +bool areSame(double a, double b) { return fabs(a - b) < std::numeric_limits::epsilon(); } void testConvertFromImagePointToPoint2f() { @@ -90,8 +87,7 @@ void testConvertFromImagePointToPoint2f() 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)) + if (areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++; @@ -100,8 +96,7 @@ void testConvertFromImagePointToPoint2f() nbNOk += 3; } - std::cout << "testConvertFromImagePointToPoint2f=" << nbOk << "/" - << (nbOk + nbNOk) << std::endl; + std::cout << "testConvertFromImagePointToPoint2f=" << nbOk << "/" << (nbOk + nbNOk) << std::endl; #endif } @@ -140,8 +135,7 @@ void testConvertFromPoint2fToImagePoint() 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)) + if (areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++; @@ -150,8 +144,7 @@ void testConvertFromPoint2fToImagePoint() nbNOk += 3; } - std::cout << "testConvertFromPoint2fToImagePoint=" << nbOk << "/" - << (nbOk + nbNOk) << std::endl; + std::cout << "testConvertFromPoint2fToImagePoint=" << nbOk << "/" << (nbOk + nbNOk) << std::endl; #endif } @@ -191,8 +184,7 @@ void testConvertFromImagePointToPoint2d() 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)) + if (areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++; @@ -201,8 +193,7 @@ void testConvertFromImagePointToPoint2d() nbNOk += 3; } - std::cout << "testConvertFromImagePointToPoint2d=" << nbOk << "/" - << (nbOk + nbNOk) << std::endl; + std::cout << "testConvertFromImagePointToPoint2d=" << nbOk << "/" << (nbOk + nbNOk) << std::endl; #endif } @@ -241,8 +232,7 @@ void testConvertFromPoint2dToImagePoint() 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)) + if (areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++; @@ -251,16 +241,14 @@ void testConvertFromPoint2dToImagePoint() nbNOk += 3; } - std::cout << "testConvertFromPoint2dToImagePoint=" << nbOk << "/" - << (nbOk + nbNOk) << std::endl; + 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); + 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); @@ -301,16 +289,14 @@ void testConvertFromKeyPointToImagePoint() nbNOk += 3; } - std::cout << "testConvertFromKeyPointToImagePoint=" << nbOk << "/" - << (nbOk + nbNOk) << std::endl; + 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); + 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); @@ -318,18 +304,15 @@ void testConvertFromPoint3fToPoint() 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())) + 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())) + 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())) + if (areSame(pt3.x, point3.get_oX()) && areSame(pt3.y, point3.get_oY()) && areSame(pt3.z, point3.get_oZ())) nbOk++; else nbNOk++; @@ -344,8 +327,7 @@ void testConvertFromPoint3fToPoint() 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) && + 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 @@ -355,8 +337,7 @@ void testConvertFromPoint3fToPoint() nbNOk += 3; } - std::cout << "testConvertFromPoint3fToPoint=" << nbOk << "/" - << (nbOk + nbNOk) << std::endl; + std::cout << "testConvertFromPoint3fToPoint=" << nbOk << "/" << (nbOk + nbNOk) << std::endl; #endif } @@ -382,18 +363,15 @@ void testConvertFromPointToPoint3f() 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())) + 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())) + 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())) + if (areSame(pt3.x, point3.get_oX()) && areSame(pt3.y, point3.get_oY()) && areSame(pt3.z, point3.get_oZ())) nbOk++; else nbNOk++; @@ -408,8 +386,7 @@ void testConvertFromPointToPoint3f() 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) && + 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 @@ -419,8 +396,7 @@ void testConvertFromPointToPoint3f() nbNOk += 3; } - std::cout << "testConvertFromPointToPoint3f=" << nbOk << "/" - << (nbOk + nbNOk) << std::endl; + std::cout << "testConvertFromPointToPoint3f=" << nbOk << "/" << (nbOk + nbNOk) << std::endl; #endif } diff --git a/modules/core/test/tools/cpu-features/testCPUFeatures.cpp b/modules/core/test/tools/cpu-features/testCPUFeatures.cpp index 0f51efa3a8..c125a07c3a 100644 --- a/modules/core/test/tools/cpu-features/testCPUFeatures.cpp +++ b/modules/core/test/tools/cpu-features/testCPUFeatures.cpp @@ -37,8 +37,7 @@ #include #include -#if defined __SSE2__ || defined _M_X64 || \ - (defined _M_IX86_FP && _M_IX86_FP >= 2) +#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2) #include #define VISP_HAVE_SSE2 1 @@ -66,12 +65,12 @@ int main() { vpCPUFeatures::printCPUInfo(); - std::cout << "checkSSE2: " << vpCPUFeatures::checkSSE2() - << " ; VISP_HAVE_SSE2: " << VALUE(VISP_HAVE_SSE2) << std::endl; - std::cout << "checkSSE3: " << vpCPUFeatures::checkSSE3() - << " ; VISP_HAVE_SSE3: " << VALUE(VISP_HAVE_SSE3) << std::endl; - std::cout << "checkSSSE3: " << vpCPUFeatures::checkSSSE3() - << " ; VISP_HAVE_SSSE3: " << VALUE(VISP_HAVE_SSSE3) << std::endl; + std::cout << "checkSSE2: " << vpCPUFeatures::checkSSE2() << " ; VISP_HAVE_SSE2: " << VALUE(VISP_HAVE_SSE2) + << std::endl; + std::cout << "checkSSE3: " << vpCPUFeatures::checkSSE3() << " ; VISP_HAVE_SSE3: " << VALUE(VISP_HAVE_SSE3) + << std::endl; + std::cout << "checkSSSE3: " << vpCPUFeatures::checkSSSE3() << " ; VISP_HAVE_SSSE3: " << VALUE(VISP_HAVE_SSSE3) + << std::endl; return EXIT_SUCCESS; } diff --git a/modules/core/test/tools/geometry/testPolygon.cpp b/modules/core/test/tools/geometry/testPolygon.cpp index 957318112f..4e19c7aecf 100644 --- a/modules/core/test/tools/geometry/testPolygon.cpp +++ b/modules/core/test/tools/geometry/testPolygon.cpp @@ -55,8 +55,7 @@ #define GETOPTARGS "cdm:h" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &opt_display, - bool &opt_click, int &method); +bool getOptions(int argc, const char **argv, bool &opt_display, bool &opt_click, int &method); /*! @@ -104,8 +103,7 @@ OPTIONS: \n\ \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, int &method) +bool getOptions(int argc, const char **argv, bool &opt_display, bool &opt_click, int &method) { const char *optarg_; int c; @@ -205,8 +203,7 @@ int main(int argc, const char **argv) std::cout << " center : " << p3.getCenter() << std::endl; if (opt_display) { -#if (defined VISP_HAVE_X11) || (defined VISP_HAVE_GTK) || \ - (defined VISP_HAVE_GDI) +#if (defined VISP_HAVE_X11) || (defined VISP_HAVE_GTK) || (defined VISP_HAVE_GDI) display.init(I, 10, 10, "Test vpPolygon"); #endif vpDisplay::display(I); @@ -216,19 +213,15 @@ int main(int argc, const char **argv) 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::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::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; @@ -243,12 +236,9 @@ int main(int argc, const char **argv) 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), - (vpPolygon::PointInPolygonMethod)method)) { + 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), (vpPolygon::PointInPolygonMethod)method)) { vpDisplay::displayPoint(I, vpImagePoint(i, j), vpColor::orange); } } @@ -259,9 +249,7 @@ int main(int argc, const char **argv) vpDisplay::getClick(I); for (unsigned int i = 0; i < I.getHeight(); ++i) { for (unsigned int j = 0; j < I.getWidth(); ++j) { - if (vpPolygon::isInside( - p4.getCorners(), i, j, - (vpPolygon::PointInPolygonMethod)method)) { + if (vpPolygon::isInside(p4.getCorners(), i, j, (vpPolygon::PointInPolygonMethod)method)) { vpDisplay::displayPoint(I, vpImagePoint(i, j), vpColor::green); } } @@ -283,31 +271,25 @@ int main(int argc, const char **argv) double t_benchmark = vpTime::measureTimeMs(); for (unsigned int i = 0; i < I_segmentIntersection.getHeight(); i++) { - for (unsigned int j = 0; j < I_segmentIntersection.getWidth(); - j++) { - if (polygon_benchmark.isInside( - vpImagePoint(i, j), - vpPolygon::PnPolySegmentIntersection)) { + for (unsigned int j = 0; j < I_segmentIntersection.getWidth(); j++) { + if (polygon_benchmark.isInside(vpImagePoint(i, j), vpPolygon::PnPolySegmentIntersection)) { I_segmentIntersection[i][j] = 255; } } } t_benchmark = vpTime::measureTimeMs() - t_benchmark; - std::cout << "PnPolySegmentIntersection: " << t_benchmark << " ms" - << std::endl; + std::cout << "PnPolySegmentIntersection: " << t_benchmark << " ms" << std::endl; t_benchmark = vpTime::measureTimeMs(); for (unsigned int i = 0; i < I_rayCasting.getHeight(); i++) { for (unsigned int j = 0; j < I_rayCasting.getWidth(); j++) { - if (polygon_benchmark.isInside(vpImagePoint(i, j), - vpPolygon::PnPolyRayCasting)) { + if (polygon_benchmark.isInside(vpImagePoint(i, j), vpPolygon::PnPolyRayCasting)) { I_rayCasting[i][j] = 255; } } } t_benchmark = vpTime::measureTimeMs() - t_benchmark; - std::cout << "PnPolyRayCasting: " << t_benchmark << " ms" - << std::endl; + std::cout << "PnPolyRayCasting: " << t_benchmark << " ms" << std::endl; #if defined VISP_HAVE_X11 vpDisplayX display1, display2; @@ -317,19 +299,14 @@ int main(int argc, const char **argv) vpDisplayGDI display1, display2; #endif -#if (defined VISP_HAVE_X11) || (defined VISP_HAVE_GTK) || \ - (defined VISP_HAVE_GDI) - display1.init(I_segmentIntersection, 10, 10, - "Segment Intersection test"); - display2.init(I_rayCasting, - (int)I_segmentIntersection.getWidth() + 10, 10, - "Ray Casting test"); +#if (defined VISP_HAVE_X11) || (defined VISP_HAVE_GTK) || (defined VISP_HAVE_GDI) + display1.init(I_segmentIntersection, 10, 10, "Segment Intersection test"); + display2.init(I_rayCasting, (int)I_segmentIntersection.getWidth() + 10, 10, "Ray Casting test"); #endif vpDisplay::display(I_segmentIntersection); vpDisplay::display(I_rayCasting); - vpDisplay::displayText(I_rayCasting, 20, 20, "Click to quit.", - vpColor::red); + vpDisplay::displayText(I_rayCasting, 20, 20, "Click to quit.", vpColor::red); vpDisplay::flush(I_segmentIntersection); vpDisplay::flush(I_rayCasting); diff --git a/modules/core/test/tools/geometry/testRect.cpp b/modules/core/test/tools/geometry/testRect.cpp index ddf75e03a0..029c360da1 100644 --- a/modules/core/test/tools/geometry/testRect.cpp +++ b/modules/core/test/tools/geometry/testRect.cpp @@ -40,10 +40,8 @@ int main() { vpRect c(10.1, 15.05, 19.63, 7.84); - vpRect a(c.getLeft() - 12.456, c.getTop() - 7.75, c.getWidth() + 12.456, - c.getHeight() + 7.75); - vpRect b(c.getLeft(), c.getTop(), c.getWidth() + 8.81, - c.getHeight() + 14.57); + vpRect a(c.getLeft() - 12.456, c.getTop() - 7.75, c.getWidth() + 12.456, c.getHeight() + 7.75); + vpRect b(c.getLeft(), c.getTop(), c.getWidth() + 8.81, c.getHeight() + 14.57); vpRect intersect = a & b; diff --git a/modules/core/test/tools/histogram/testHistogram.cpp b/modules/core/test/tools/histogram/testHistogram.cpp index df3e8d0c62..fe68d0bec6 100644 --- a/modules/core/test/tools/histogram/testHistogram.cpp +++ b/modules/core/test/tools/histogram/testHistogram.cpp @@ -102,8 +102,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, std::string &ipath, - unsigned int &nbThreads) +bool getOptions(int argc, const char **argv, std::string &ipath, unsigned int &nbThreads) { const char *optarg_; int c; @@ -150,9 +149,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, \param nbBins : Number of histogram bins. \param nbThreads : Number of computation threads. */ -unsigned int histogramSum(const vpImage &I, - const unsigned int nbBins, - const unsigned int nbThreads) +unsigned int histogramSum(const vpImage &I, const unsigned int nbBins, const unsigned int nbThreads) { unsigned int sum = 0; @@ -172,8 +169,7 @@ unsigned int histogramSum(const vpImage &I, \param I : Input color image. \param nbBins : Number of histogram bins. */ -bool compareHistogram(const vpImage &I, - const unsigned int nbBins) +bool compareHistogram(const vpImage &I, const unsigned int nbBins) { vpHistogram histogram_single_threaded; histogram_single_threaded.calculate(I, nbBins, 1); @@ -184,10 +180,8 @@ bool compareHistogram(const vpImage &I, unsigned int sum = 0; for (unsigned int cpt = 0; cpt < nbBins; cpt++) { if (histogram_single_threaded[cpt] != histogram_multi_threaded[cpt]) { - std::cerr << "histogram_single_threaded[" << cpt - << "]=" << histogram_single_threaded[cpt] - << " ; histogram_multi_threaded[" << cpt - << "]=" << histogram_multi_threaded[cpt] << std::endl; + std::cerr << "histogram_single_threaded[" << cpt << "]=" << histogram_single_threaded[cpt] + << " ; histogram_multi_threaded[" << cpt << "]=" << histogram_multi_threaded[cpt] << std::endl; return false; } @@ -196,8 +190,7 @@ bool compareHistogram(const vpImage &I, } if (sum != I.getSize()) { - std::cerr << "Sum of histogram is different with the image size!" - << std::endl; + std::cerr << "Sum of histogram is different with the image size!" << std::endl; return false; } @@ -236,8 +229,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -246,13 +238,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -287,19 +276,13 @@ int main(int argc, const char **argv) } t_multithread = vpTime::measureTimeMs() - t_multithread; - std::cout << "sum_single_thread=" << sum_single_thread - << " ; t_single_thread=" << t_single_thread - << " ms ; mean=" << t_single_thread / (double)nbIterations - << " ms" << std::endl; - std::cout << "sum_single_multithread=" << sum_single_multithread - << " ; t_multithread=" << t_multithread - << " ms ; mean=" << t_multithread / (double)nbIterations - << " ms" << std::endl; - std::cout << "Speed-up=" << t_single_thread / (double)t_multithread << "X" - << std::endl; - - if (sum_single_thread != I.getSize() || - sum_single_multithread != I.getSize()) { + std::cout << "sum_single_thread=" << sum_single_thread << " ; t_single_thread=" << t_single_thread + << " ms ; mean=" << t_single_thread / (double)nbIterations << " ms" << std::endl; + std::cout << "sum_single_multithread=" << sum_single_multithread << " ; t_multithread=" << t_multithread + << " ms ; mean=" << t_multithread / (double)nbIterations << " ms" << std::endl; + std::cout << "Speed-up=" << t_single_thread / (double)t_multithread << "X" << std::endl; + + if (sum_single_thread != I.getSize() || sum_single_multithread != I.getSize()) { std::cerr << "Problem with histogram!" << std::endl; return -1; } @@ -317,9 +300,8 @@ int main(int argc, const char **argv) if (histogram.getSize() == 256) { for (unsigned int cpt = 0; cpt < 256; cpt++) { if (histogram[cpt] != 0) { - std::cerr << "Problem with histogram computation: histogram[" << cpt - << "]=" << histogram[cpt] << " but should be zero!" - << std::endl; + std::cerr << "Problem with histogram computation: histogram[" << cpt << "]=" << histogram[cpt] + << " but should be zero!" << std::endl; } } } else { @@ -335,15 +317,13 @@ int main(int argc, const char **argv) for (unsigned int cpt = 0; cpt < 256; cpt++) { if (cpt == 100) { if (histogram[cpt] != I_test.getSize()) { - std::cerr << "Problem with histogram computation: histogram[" - << cpt << "]=" << histogram[cpt] + std::cerr << "Problem with histogram computation: histogram[" << cpt << "]=" << histogram[cpt] << " but should be: " << I_test.getSize() << std::endl; return -1; } } else { if (histogram[cpt] != 0) { - std::cerr << "Problem with histogram computation: histogram[" - << cpt << "]=" << histogram[cpt] + std::cerr << "Problem with histogram computation: histogram[" << cpt << "]=" << histogram[cpt] << " but should be zero!" << std::endl; } } @@ -361,15 +341,13 @@ int main(int argc, const char **argv) for (unsigned int cpt = 0; cpt < 256; cpt++) { if (cpt == 50) { if (histogram[cpt] != I_test.getSize()) { - std::cerr << "Problem with histogram computation: histogram[" - << cpt << "]=" << histogram[cpt] + std::cerr << "Problem with histogram computation: histogram[" << cpt << "]=" << histogram[cpt] << " but should be: " << I_test.getSize() << std::endl; return -1; } } else { if (histogram[cpt] != 0) { - std::cerr << "Problem with histogram computation: histogram[" - << cpt << "]=" << histogram[cpt] + std::cerr << "Problem with histogram computation: histogram[" << cpt << "]=" << histogram[cpt] << " but should be zero!" << std::endl; } } diff --git a/modules/core/test/tools/io/testIoTools.cpp b/modules/core/test/tools/io/testIoTools.cpp index fe2e78261a..e0b0360657 100644 --- a/modules/core/test/tools/io/testIoTools.cpp +++ b/modules/core/test/tools/io/testIoTools.cpp @@ -51,11 +51,9 @@ 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; + 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::cout << "The directory separator character is '" << c << "' (Unix like platform)." << std::endl; } #if defined(_WIN32) @@ -64,240 +62,174 @@ int main(int argc, const char **argv) std::string 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; + 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::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::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 " + 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; + 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; + 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 << "pathname=" << vpIoTools::splitDrive(pathname).first << " ; " << vpIoTools::splitDrive(pathname).second + << std::endl; - std::cout << "isAbsolutePath of " << pathname << "=" - << vpIoTools::isAbsolutePathname(pathname) << 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; + 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; + 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; + 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; + 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) { + 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; + 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) { + 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; + 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; + 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) { + 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; + 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) { + 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; + 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) { + 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; + 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) { + 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; + 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) { + 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; + 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) { + 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; + 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(), + 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 << "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) { + 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; + 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) { + 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 + 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) { + 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; + 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(), + 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 << "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) { + 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; + 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) { + 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 + 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; + std::cout << "Test vpIoTools::splitDrive (Win32) - passed: " << nbOk << "/" << (nbOk + nbFail) << std::endl; if (nbFail) { std::cerr << "Failed test: vpIoTools::splitDrive (Win32)" << std::endl; @@ -314,93 +246,77 @@ int main(int argc, const char **argv) nbOk++; } else { nbFail++; - std::cout << "Fail=" << vpIoTools::getFileExtension("foo.ext") - << " should be=.ext" << std::endl; + std::cout << "Fail=" << vpIoTools::getFileExtension("foo.ext") << " should be=.ext" << std::endl; } - if (strcmp(vpIoTools::getFileExtension("/foo/foo.ext").c_str(), ".ext") == - 0) { + 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; + 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; + std::cout << "Fail=" << vpIoTools::getFileExtension(".ext") << " should be=" << std::endl; } - if (strcmp(vpIoTools::getFileExtension("\\foo.ext\\foo").c_str(), "") == - 0) { + 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; + 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; + 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; + std::cout << "Fail=" << vpIoTools::getFileExtension("") << " should be=" << std::endl; } - if (strcmp(vpIoTools::getFileExtension("foo.bar.ext").c_str(), ".ext") == - 0) { + 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; + 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) { + 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; + 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) { + 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; + 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 << "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; + std::cout << "Test vpIoTools::getFileExtension (WIN32 platform) - passed: " << nbOk << "/" << (nbOk + nbFail) + << std::endl; if (nbFail) { - std::cerr << "Failed test: vpIoTools::getFileExtension (WIN32 platform)" - << std::endl; + std::cerr << "Failed test: vpIoTools::getFileExtension (WIN32 platform)" << std::endl; return EXIT_FAILURE; } #else @@ -411,104 +327,88 @@ int main(int argc, const char **argv) nbOk++; } else { nbFail++; - std::cout << "Fail=" << vpIoTools::getFileExtension("foo.bar") - << " should be=.bar" << std::endl; + std::cout << "Fail=" << vpIoTools::getFileExtension("foo.bar") << " should be=.bar" << std::endl; } - if (strcmp(vpIoTools::getFileExtension("foo.boo.bar").c_str(), ".bar") == - 0) { + 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; + 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) { + 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; + 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; + 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; + 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; + 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; + std::cout << "Fail=" << vpIoTools::getFileExtension("...manydots") << " should be=" << std::endl; } - if (strcmp(vpIoTools::getFileExtension("...manydots.ext").c_str(), - ".ext") == 0) { + 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; + 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; + 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 << "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 << "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 << "Fail=" << vpIoTools::getFileExtension("") << " should be=" << std::endl; } - std::cout - << "Test vpIoTools::getFileExtension (Unix-like platform) - passed: " - << nbOk << "/" << (nbOk + nbFail) << std::endl; + std::cout << "Test vpIoTools::getFileExtension (Unix-like platform) - passed: " << nbOk << "/" << (nbOk + nbFail) + << std::endl; #endif // Test makeDirectory() @@ -516,60 +416,42 @@ int main(int argc, const char **argv) std::string username, directory_filename; vpIoTools::getUserName(username); #if defined(_WIN32) - directory_filename = - "C:/temp/" + username + "/test_directory1/test directory 2/"; -#elif (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) - directory_filename = - "/tmp/" + username + "/test_directory1/test directory 2/"; + directory_filename = "C:/temp/" + username + "/test_directory1/test directory 2/"; +#elif (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) + directory_filename = "/tmp/" + username + "/test_directory1/test directory 2/"; #endif vpIoTools::makeDirectory(directory_filename); vpIoTools::makeDirectory(directory_filename); std::cout << "Create directories: " << directory_filename - << " ; check: " << vpIoTools::checkDirectory(directory_filename) - << std::endl; + << " ; check: " << vpIoTools::checkDirectory(directory_filename) << std::endl; #if defined(_WIN32) - directory_filename = - "C:/temp/" + username + "/test_directory1/test directory 3"; -#elif (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) - directory_filename = - "/tmp/" + username + "/test_directory1/test directory 3"; + directory_filename = "C:/temp/" + username + "/test_directory1/test directory 3"; +#elif (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) + directory_filename = "/tmp/" + username + "/test_directory1/test directory 3"; #endif vpIoTools::makeDirectory(directory_filename); std::cout << "Create directories: " << directory_filename - << " ; check: " << vpIoTools::checkDirectory(directory_filename) - << std::endl; + << " ; check: " << vpIoTools::checkDirectory(directory_filename) << std::endl; #if defined(_WIN32) - directory_filename = - "C:\\temp/" + username + "\\test_directory1\\test directory 4"; -#elif (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) - directory_filename = - "/tmp\\" + username + "\\test_directory1\\test directory 4"; + directory_filename = "C:\\temp/" + username + "\\test_directory1\\test directory 4"; +#elif (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) + directory_filename = "/tmp\\" + username + "\\test_directory1\\test directory 4"; #endif vpIoTools::makeDirectory(directory_filename); vpIoTools::makeDirectory(directory_filename); std::cout << "Create directories: " << directory_filename - << " ; check: " << vpIoTools::checkDirectory(directory_filename) - << std::endl; + << " ; check: " << vpIoTools::checkDirectory(directory_filename) << std::endl; #if defined(_WIN32) - directory_filename = - "C:\\temp/" + username + - "\\test_directory1\\test directory 5 . dir/test directory 6"; -#elif (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) - directory_filename = - "/tmp\\" + username + - "\\test_directory1\\test directory 5 . dir/test directory 6"; + directory_filename = "C:\\temp/" + username + "\\test_directory1\\test directory 5 . dir/test directory 6"; +#elif (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) + directory_filename = "/tmp\\" + username + "\\test_directory1\\test directory 5 . dir/test directory 6"; #endif vpIoTools::makeDirectory(directory_filename); std::cout << "Create directories: " << directory_filename - << " ; check: " << vpIoTools::checkDirectory(directory_filename) - << std::endl; + << " ; check: " << vpIoTools::checkDirectory(directory_filename) << std::endl; } catch (const vpException &e) { std::cerr << "Exception: " << e.what() << std::endl; return EXIT_FAILURE; @@ -590,37 +472,32 @@ int main(int argc, const char **argv) bool res; res = vpIoTools::isSamePathname(path1, path2); // True - std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " - << res << std::endl; + std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl; nbOk = res ? nbOk + 1 : nbOk; nbFail = res ? nbFail : nbFail + 1; path1 = ".\\tmp/test/file.txt"; res = vpIoTools::isSamePathname(path1, path2); // True - std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " - << res << std::endl; + std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl; nbOk = res ? nbOk + 1 : nbOk; nbFail = res ? nbFail : nbFail + 1; path1 = ".\\tmp/test\\../fake dir/..\\test\\file.txt"; res = vpIoTools::isSamePathname(path1, path2); // True - std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " - << res << std::endl; + std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl; nbOk = res ? nbOk + 1 : nbOk; nbFail = res ? nbFail : nbFail + 1; path2 = "/tmp/test/../test/file.txt"; res = vpIoTools::isSamePathname(path1, path2); // False - std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " - << res << std::endl; + std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl; nbOk = res ? nbOk : nbOk + 1; nbFail = res ? nbFail + 1 : nbFail; - std::cout << "Test vpIoTools::isSamePathname (WIN32 platform) - passed: " - << nbOk << "/" << (nbOk + nbFail) << std::endl; + std::cout << "Test vpIoTools::isSamePathname (WIN32 platform) - passed: " << nbOk << "/" << (nbOk + nbFail) + << std::endl; if (nbFail) { - std::cerr << "Failed test: vpIoTools::isSamePathname (WIN32 platform)" - << std::endl; + std::cerr << "Failed test: vpIoTools::isSamePathname (WIN32 platform)" << std::endl; return EXIT_FAILURE; } #else @@ -642,51 +519,44 @@ int main(int argc, const char **argv) bool res; res = vpIoTools::isSamePathname(path1, path2); // True - std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " - << res << std::endl; + std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl; nbOk = res ? nbOk + 1 : nbOk; nbFail = res ? nbFail : nbFail + 1; path1 = "\\tmp/" + username + "/./test/file.txt"; res = vpIoTools::isSamePathname(path1, path2); // True - std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " - << res << std::endl; + std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl; nbOk = res ? nbOk + 1 : nbOk; nbFail = res ? nbFail : nbFail + 1; path1 = "\\tmp/" + username + "/test\\../dummy dir/..\\test\\file.txt"; res = vpIoTools::isSamePathname(path1, path2); // True - std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " - << res << std::endl; + std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl; nbOk = res ? nbOk + 1 : nbOk; nbFail = res ? nbFail : nbFail + 1; path2 = "/tmp/" + username + "/test/../test"; res = vpIoTools::isSamePathname(path1, path2); // False - std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " - << res << std::endl; + std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl; nbOk = res ? nbOk : nbOk + 1; nbFail = res ? nbFail + 1 : nbFail; path1 = "/tmp/" + username + "/test/"; res = vpIoTools::isSamePathname(path1, path2); // True - std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " - << res << std::endl; + std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl; nbOk = res ? nbOk + 1 : nbOk; nbFail = res ? nbFail : nbFail + 1; - std::cout << "Test vpIoTools::isSamePathname (Unix platform) - passed: " - << nbOk << "/" << (nbOk + nbFail) << std::endl; + std::cout << "Test vpIoTools::isSamePathname (Unix platform) - passed: " << nbOk << "/" << (nbOk + nbFail) + << std::endl; if (nbFail) { - std::cerr << "Failed test: vpIoTools::isSamePathname (Unix platform)" - << std::endl; + std::cerr << "Failed test: vpIoTools::isSamePathname (Unix platform)" << std::endl; return EXIT_FAILURE; } #endif // Test checkFilename() - vpIoTools::makeDirectory("/tmp/" + username + - "/directory (1) with ' quote and spaces"); + vpIoTools::makeDirectory("/tmp/" + username + "/directory (1) with ' quote and spaces"); path1 = "/tmp/" + username + "/directory (1) with ' quote and spaces/file with ' quote (1) and " "spaces.txt"; diff --git a/modules/core/test/tools/threading/testMutex.cpp b/modules/core/test/tools/threading/testMutex.cpp index 82b2bca2fa..afc480391c 100644 --- a/modules/core/test/tools/threading/testMutex.cpp +++ b/modules/core/test/tools/threading/testMutex.cpp @@ -59,8 +59,7 @@ vpThread::Return doSomeThing(vpThread::Args args) mutex.lock(); unsigned int thread_id = *((unsigned int *)args); - std::cout << "Started job " << thread_counter << " with id " << thread_id - << std::endl; + std::cout << "Started job " << thread_counter << " with id " << thread_id << std::endl; for (unsigned long i = 0; i < (0xFFFF); i++) { }; @@ -81,8 +80,7 @@ int main(void) for (unsigned int i = 0; i < nthread; i++) { thread_id[i] = i; - thread[i].create((vpThread::Fn)&doSomeThing, - (vpThread::Args)&thread_id[i]); + thread[i].create((vpThread::Fn)&doSomeThing, (vpThread::Args)&thread_id[i]); } delete[] thread; @@ -94,13 +92,10 @@ int main(void) #else int main() { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX - std::cout << "You should enable pthread usage and rebuild ViSP..." - << std::endl; +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX + std::cout << "You should enable pthread usage and rebuild ViSP..." << std::endl; #else - std::cout << "Multi-threading seems not supported on this platform" - << std::endl; + std::cout << "Multi-threading seems not supported on this platform" << std::endl; #endif } #endif diff --git a/modules/core/test/tools/threading/testThread.cpp b/modules/core/test/tools/threading/testThread.cpp index 8d57dfb38d..cae484a728 100644 --- a/modules/core/test/tools/threading/testThread.cpp +++ b/modules/core/test/tools/threading/testThread.cpp @@ -81,9 +81,8 @@ int main() unsigned int qux_arg = 12; vpThread foo; vpThread bar((vpThread::Fn)myBarFunction); - vpThread qux( - (vpThread::Fn)myQuxFunction, - (vpThread::Args)&qux_arg); // Pass qux_arg to myQuxFunction() function + vpThread qux((vpThread::Fn)myQuxFunction, + (vpThread::Args)&qux_arg); // Pass qux_arg to myQuxFunction() function vpTime::wait(1000); // Sleep 1s to ensure myQuxFunction() internal printings std::cout << "Joinable after construction:" << std::endl; @@ -120,13 +119,10 @@ int main() int main() { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX - std::cout << "You should enable pthread usage and rebuild ViSP..." - << std::endl; +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX + std::cout << "You should enable pthread usage and rebuild ViSP..." << std::endl; #else - std::cout << "Multi-threading seems not supported on this platform" - << std::endl; + std::cout << "Multi-threading seems not supported on this platform" << std::endl; #endif } #endif diff --git a/modules/core/test/tools/threading/testThread2.cpp b/modules/core/test/tools/threading/testThread2.cpp index ad427729f2..20eab1f7fd 100644 --- a/modules/core/test/tools/threading/testThread2.cpp +++ b/modules/core/test/tools/threading/testThread2.cpp @@ -57,17 +57,12 @@ namespace class ArithmFunctor { public: - ArithmFunctor(const vpColVector &v1, const vpColVector &v2, - const unsigned int start, const unsigned int end) - : m_add(), m_mul(), m_v1(v1), m_v2(v2), m_indexStart(start), - m_indexEnd(end) + ArithmFunctor(const vpColVector &v1, const vpColVector &v2, const unsigned int start, const unsigned int end) + : m_add(), m_mul(), m_v1(v1), m_v2(v2), m_indexStart(start), m_indexEnd(end) { } - ArithmFunctor() - : m_add(), m_mul(), m_v1(), m_v2(), m_indexStart(0), m_indexEnd(0) - { - } + ArithmFunctor() : m_add(), m_mul(), m_v1(), m_v2(), m_indexStart(0), m_indexEnd(0) {} void operator()() { computeImpl(); } @@ -90,8 +85,7 @@ class ArithmFunctor // to simulate a long computation for (int iter = 0; iter < 100; iter++) { - for (unsigned int i = m_indexStart, cpt = 0; i < m_indexEnd; - i++, cpt++) { + for (unsigned int i = m_indexStart, cpt = 0; i < m_indexEnd; i++, cpt++) { m_add[cpt] = m_v1[i] + m_v2[i]; m_mul[cpt] = m_v1[i] * m_v2[i]; } @@ -119,8 +113,7 @@ void insert(vpColVector &v1, const vpColVector &v2) } } -bool check(const vpColVector &v1, const vpColVector &v2, - const vpColVector &res_add, const vpColVector &res_mul) +bool check(const vpColVector &v1, const vpColVector &v2, const vpColVector &res_add, const vpColVector &res_mul) { double add = 0.0, mul = 0.0; for (unsigned int i = 0; i < v1.size(); i++) { @@ -135,14 +128,12 @@ bool check(const vpColVector &v1, const vpColVector &v2, std::cout << "mul=" << mul << " ; mul_th=" << mul_th << std::endl; if (!vpMath::equal(add, add_th, std::numeric_limits::epsilon())) { - std::cerr << "Problem: add=" << add << " ; add_th=" << add_th - << std::endl; + std::cerr << "Problem: add=" << add << " ; add_th=" << add_th << std::endl; return false; } if (!vpMath::equal(mul, mul_th, std::numeric_limits::epsilon())) { - std::cerr << "Problem: mul=" << mul << " ; mul_th=" << mul_th - << std::endl; + std::cerr << "Problem: mul=" << mul << " ; mul_th=" << mul_th << std::endl; return false; } @@ -181,8 +172,7 @@ int main() } std::cout << "Create thread: " << i << std::endl; - threads[i].create((vpThread::Fn)arithmThread, - (vpThread::Args)&functors[i]); + threads[i].create((vpThread::Fn)arithmThread, (vpThread::Args)&functors[i]); } //! [functor-thread-example threadCreation] @@ -213,13 +203,10 @@ int main() int main() { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX - std::cout << "You should enable pthread usage and rebuild ViSP..." - << std::endl; +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX + std::cout << "You should enable pthread usage and rebuild ViSP..." << std::endl; #else - std::cout << "Multi-threading seems not supported on this platform" - << std::endl; + std::cout << "Multi-threading seems not supported on this platform" << std::endl; #endif return EXIT_SUCCESS; } diff --git a/modules/core/test/tools/time/testTime.cpp b/modules/core/test/tools/time/testTime.cpp index 8ccfc65fcc..a582e5de8a 100644 --- a/modules/core/test/tools/time/testTime.cpp +++ b/modules/core/test/tools/time/testTime.cpp @@ -44,8 +44,7 @@ */ #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #include #elif defined(_WIN32) //#include @@ -78,8 +77,7 @@ int main() double t2 = vpTime::measureTimeMs(); // Sleep 10ms -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX usleep(10 * 1000); #elif defined(_WIN32) Sleep(10); @@ -88,8 +86,7 @@ int main() double t3 = vpTime::measureTimeMs(); // Sleep 2ms -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX usleep(2 * 1000); #elif defined(_WIN32) Sleep(2); @@ -128,7 +125,6 @@ int main() return 1; } #else - std::cout << "vpTime is not implemented on Universal Windows Platform" - << std::endl; + std::cout << "vpTime is not implemented on Universal Windows Platform" << std::endl; #endif } diff --git a/modules/core/test/tools/xml/testXmlParser.cpp b/modules/core/test/tools/xml/testXmlParser.cpp index b517c5d607..6012b4f260 100644 --- a/modules/core/test/tools/xml/testXmlParser.cpp +++ b/modules/core/test/tools/xml/testXmlParser.cpp @@ -115,8 +115,7 @@ class vpExampleDataParser : public vpXmlParser default values. */ -vpExampleDataParser::vpExampleDataParser() - : m_range(0.), m_step(0), m_size_filter(0), m_name("") +vpExampleDataParser::vpExampleDataParser() : m_range(0.), m_step(0), m_size_filter(0), m_name("") { nodeMap["config"] = config; nodeMap["range"] = range; @@ -141,11 +140,9 @@ vpExampleDataParser::~vpExampleDataParser() {} */ void vpExampleDataParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) { - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case range: @@ -161,8 +158,7 @@ void vpExampleDataParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) this->m_name = xmlReadStringChild(doc, dataNode); } break; default: - vpTRACE("unknown tag in readConfigNode : %d, %s", iter_data->second, - (iter_data->first).c_str()); + vpTRACE("unknown tag in readConfigNode : %d, %s", iter_data->second, (iter_data->first).c_str()); break; } } @@ -196,10 +192,8 @@ void vpExampleDataParser::writeMainClass(xmlNodePtr node) // 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); +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); /*! @@ -211,8 +205,7 @@ Print the program options. \param user : Username. */ -void usage(const char *name, const char *badparam, const std::string &opath, - const std::string &user) +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\ @@ -246,8 +239,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &opath, const std::string &user) { const char *optarg_; int c; @@ -298,17 +290,14 @@ int main(int argc, const char **argv) std::string filename; std::string username; - std::cout << "-------------------------------------------------------" - << std::endl; + 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; std::cout << std::endl; // Set the default output path -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX opt_opath = "/tmp"; #elif defined(_WIN32) opt_opath = "C:\\temp"; diff --git a/modules/detection/include/visp3/detection/vpDetectorAprilTag.h b/modules/detection/include/visp3/detection/vpDetectorAprilTag.h index 7486406a52..9b8871e0c4 100644 --- a/modules/detection/include/visp3/detection/vpDetectorAprilTag.h +++ b/modules/detection/include/visp3/detection/vpDetectorAprilTag.h @@ -51,7 +51,8 @@ href="https://april.eecs.umich.edu/software/apriltag.html">AprilTag. There is no need to download and install AprilTag from source code or existing pre-built packages since the source code is embedded in ViSP. Reference papers are AprilTag: A robust and flexible visual fiducial system -(\cite olson2011tags) and AprilTag 2: Efficient and robust fiducial detection +(\cite olson2011tags) and AprilTag 2: Efficient and robust fiducial +detection (\cite wang2016iros). The detect() function allows to detect multiple tags in an image. Once @@ -66,8 +67,10 @@ also to estimate the 3D pose of the tag in terms of position and orientation wrt the camera. The following sample code shows how to use this class to detect the location -of 36h11 AprilTag patterns in an image. \code #include - #include +of 36h11 AprilTag patterns in an image. +\code +#include +#include int main() { @@ -84,8 +87,7 @@ int main() std::vector 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; + std::cout << " Message: \"" << detector.getMessage(i) << "\"" << std::endl; } } #endif @@ -109,8 +111,10 @@ Tag code 1: \endcode This other example shows how to estimate the 3D pose of 36h11 AprilTag -patterns. \code #include #include - +patterns. +\code +#include +#include int main() { @@ -121,15 +125,15 @@ int main() vpDetectorAprilTag detector(vpDetectorAprilTag::TAG_36h11); std::vector cMo; vpCameraParameters cam; - cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, -243.4373779); double tagSize = 0.053; + cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779); + double tagSize = 0.053; bool status = detector.detect(I, tagSize, cam, cMo); if (status) { for(size_t i=0; i < detector.getNbObjects(); i++) { std::cout << "Tag code " << i << ":" << std::endl; - std::cout << " Message: \"" << detector.getMessage(i) << "\"" << -std::endl; std::cout << " Pose: " << vpPoseVector(cMo[i]).t() << std::endl; + std::cout << " Message: \"" << detector.getMessage(i) << "\"" << std::endl; + std::cout << " Pose: " << vpPoseVector(cMo[i]).t() << std::endl; } } #endif @@ -139,9 +143,11 @@ std::endl; std::cout << " Pose: " << vpPoseVector(cMo[i]).t() << std::endl; \code Tag code 0: Message: "36h11 id: 0" - Pose: 0.1015061088 -0.05239057228 0.3549037285 1.991474322 2.04143538 --0.9412360063 Tag code 1: Message: "36h11 id: 1" Pose: 0.08951250829 -0.02243780207 0.306540622 1.998073197 2.061488008 -0.8699567948 \endcode + Pose: 0.1015061088 -0.05239057228 0.3549037285 1.991474322 2.04143538 -0.9412360063 +Tag code 1: + Message: "36h11 id: 1" + Pose: 0.08951250829 0.02243780207 0.306540622 1.998073197 2.061488008 -0.8699567948 +\endcode Other examples are also provided in tutorial-apriltag-detector.cpp and tutorial-apriltag-detector-live.cpp @@ -183,26 +189,20 @@ class VISP_EXPORT vpDetectorAprilTag : public vpDetectorBase }; vpDetectorAprilTag(const vpAprilTagFamily &tagFamily = TAG_36h11, - const vpPoseEstimationMethod &poseEstimationMethod = - HOMOGRAPHY_VIRTUAL_VS); + const vpPoseEstimationMethod &poseEstimationMethod = HOMOGRAPHY_VIRTUAL_VS); virtual ~vpDetectorAprilTag(); bool detect(const vpImage &I); - bool detect(const vpImage &I, const double tagSize, - const vpCameraParameters &cam, + bool detect(const vpImage &I, const double tagSize, const vpCameraParameters &cam, std::vector &cMo_vec); /*! Return the pose estimation method. */ - inline vpPoseEstimationMethod getPoseEstimationMethod() const - { - return m_poseEstimationMethod; - } + inline vpPoseEstimationMethod getPoseEstimationMethod() const { return m_poseEstimationMethod; } void setAprilTagNbThreads(const int nThreads); - void setAprilTagPoseEstimationMethod( - const vpPoseEstimationMethod &poseEstimationMethod); + void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod); void setAprilTagQuadDecimate(const float quadDecimate); void setAprilTagQuadSigma(const float quadSigma); void setAprilTagRefineDecode(const bool refineDecode); @@ -227,9 +227,7 @@ class VISP_EXPORT vpDetectorAprilTag : public vpDetectorBase Impl *m_impl; }; -inline std::ostream & -operator<<(std::ostream &os, - const vpDetectorAprilTag::vpPoseEstimationMethod &method) +inline std::ostream &operator<<(std::ostream &os, const vpDetectorAprilTag::vpPoseEstimationMethod &method) { switch (method) { case vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS: @@ -255,9 +253,7 @@ operator<<(std::ostream &os, return os; } -inline std::ostream & -operator<<(std::ostream &os, - const vpDetectorAprilTag::vpAprilTagFamily &tagFamily) +inline std::ostream &operator<<(std::ostream &os, const vpDetectorAprilTag::vpAprilTagFamily &tagFamily) { switch (tagFamily) { case vpDetectorAprilTag::TAG_36h11: diff --git a/modules/detection/include/visp3/detection/vpDetectorBase.h b/modules/detection/include/visp3/detection/vpDetectorBase.h index b4a5e8d6ae..d7bd4355ef 100644 --- a/modules/detection/include/visp3/detection/vpDetectorBase.h +++ b/modules/detection/include/visp3/detection/vpDetectorBase.h @@ -68,8 +68,8 @@ class VISP_EXPORT vpDetectorBase //!< defines the polygon //!< that contains the //!< object. - std::vector m_message; //!< Message attached to each object. - size_t m_nb_objects; //!< Number of detected objects. + std::vector m_message; //!< Message attached to each object. + size_t m_nb_objects; //!< Number of detected objects. public: /*! diff --git a/modules/detection/include/visp3/detection/vpDetectorDataMatrixCode.h b/modules/detection/include/visp3/detection/vpDetectorDataMatrixCode.h index a1cce9aadb..b861b25ea5 100644 --- a/modules/detection/include/visp3/detection/vpDetectorDataMatrixCode.h +++ b/modules/detection/include/visp3/detection/vpDetectorDataMatrixCode.h @@ -63,8 +63,10 @@ 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 #include - +an image. +\code +#include +#include int main() { @@ -81,8 +83,7 @@ int main() std::vector 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; + std::cout << " Message: \"" << detector.getMessage(i) << "\"" << std::endl; } } #endif diff --git a/modules/detection/include/visp3/detection/vpDetectorFace.h b/modules/detection/include/visp3/detection/vpDetectorFace.h index f733ff6072..4db185c631 100644 --- a/modules/detection/include/visp3/detection/vpDetectorFace.h +++ b/modules/detection/include/visp3/detection/vpDetectorFace.h @@ -62,7 +62,9 @@ here https://visp.inria.fr/3rd_opencv. 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 +OpenCV. +\code +#include int main() { @@ -74,8 +76,7 @@ int main() // 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 + vpRect face_bbox = face_detector.getBoundingBox(0); // largest face has index 0 } } } @@ -87,11 +88,9 @@ provided in tutorial-face-detector-live.cpp. class VISP_EXPORT vpDetectorFace : public vpDetectorBase { protected: - std::vector 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. + std::vector 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(); diff --git a/modules/detection/include/visp3/detection/vpDetectorQRCode.h b/modules/detection/include/visp3/detection/vpDetectorQRCode.h index a13015e8e3..f045d8f8f1 100644 --- a/modules/detection/include/visp3/detection/vpDetectorQRCode.h +++ b/modules/detection/include/visp3/detection/vpDetectorQRCode.h @@ -65,8 +65,10 @@ 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 #include - +an image. +\code +#include +#include int main() { @@ -83,8 +85,7 @@ int main() std::vector 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; + std::cout << " Message: \"" << detector.getMessage(i) << "\"" << std::endl; } } #endif diff --git a/modules/detection/src/barcode/vpDetectorDataMatrixCode.cpp b/modules/detection/src/barcode/vpDetectorDataMatrixCode.cpp index 0b065f593b..a4d1bff6ed 100644 --- a/modules/detection/src/barcode/vpDetectorDataMatrixCode.cpp +++ b/modules/detection/src/barcode/vpDetectorDataMatrixCode.cpp @@ -68,8 +68,7 @@ bool vpDetectorDataMatrixCode::detect(const vpImage &I) DmtxImage *img; DmtxMessage *msg; - img = dmtxImageCreate(I.bitmap, (int)I.getWidth(), (int)I.getHeight(), - DmtxPack8bppK); + img = dmtxImageCreate(I.bitmap, (int)I.getWidth(), (int)I.getHeight(), DmtxPack8bppK); assert(img != NULL); dec = dmtxDecodeCreate(img, 1); diff --git a/modules/detection/src/barcode/vpDetectorQRCode.cpp b/modules/detection/src/barcode/vpDetectorQRCode.cpp index e3065f78d0..a358fe4dff 100644 --- a/modules/detection/src/barcode/vpDetectorQRCode.cpp +++ b/modules/detection/src/barcode/vpDetectorQRCode.cpp @@ -69,23 +69,19 @@ bool vpDetectorQRCode::detect(const vpImage &I) unsigned int height = I.getHeight(); // wrap image data - zbar::Image img(width, height, "Y800", I.bitmap, - (unsigned long)(width * height)); + 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) { + for (zbar::Image::SymbolIterator symbol = img.symbol_begin(); symbol != img.symbol_end(); ++symbol) { m_message.push_back(symbol->get_data()); detected = true; std::vector 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))); + 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); } diff --git a/modules/detection/src/face/vpDetectorFace.cpp b/modules/detection/src/face/vpDetectorFace.cpp index 7174b114d1..372db7e44f 100644 --- a/modules/detection/src/face/vpDetectorFace.cpp +++ b/modules/detection/src/face/vpDetectorFace.cpp @@ -44,17 +44,12 @@ #include #include -bool vpSortLargestFace(cv::Rect rect1, cv::Rect rect2) -{ - return (rect1.area() > rect2.area()); -} +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() -{ -} +vpDetectorFace::vpDetectorFace() : m_faces(), m_face_cascade(), m_frame_gray() {} /*! Set the name of the OpenCV cascade classifier file used for face detection. @@ -65,8 +60,7 @@ vpDetectorFace::vpDetectorFace() : m_faces(), m_face_cascade(), m_frame_gray() 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()); + throw vpException(vpException::ioError, "Cannot read haar file: %s", filename.c_str()); } } @@ -115,11 +109,9 @@ bool vpDetectorFace::detect(const cv::Mat &frame_gray) m_faces.clear(); #if (VISP_HAVE_OPENCV_VERSION >= 0x030000) - m_face_cascade.detectMultiScale(frame_gray, m_faces, 1.1, 2, 0, - cv::Size(30, 30)); + m_face_cascade.detectMultiScale(frame_gray, m_faces, 1.1, 2, 0, cv::Size(30, 30)); #else - m_face_cascade.detectMultiScale(frame_gray, m_faces, 1.1, 2, - 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30)); + m_face_cascade.detectMultiScale(frame_gray, m_faces, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30)); #endif m_nb_objects = m_faces.size(); diff --git a/modules/detection/src/tag/vpDetectorAprilTag.cpp b/modules/detection/src/tag/vpDetectorAprilTag.cpp index 764875773a..9c7b1d32da 100644 --- a/modules/detection/src/tag/vpDetectorAprilTag.cpp +++ b/modules/detection/src/tag/vpDetectorAprilTag.cpp @@ -56,10 +56,9 @@ class vpDetectorAprilTag::Impl { public: - Impl(const vpAprilTagFamily &tagFamily, - const vpPoseEstimationMethod &method) - : m_cam(), m_poseEstimationMethod(method), m_tagFamily(tagFamily), - m_tagPoses(), m_tagSize(1.0), m_td(NULL), m_tf(NULL) + Impl(const vpAprilTagFamily &tagFamily, const vpPoseEstimationMethod &method) + : m_cam(), m_poseEstimationMethod(method), m_tagFamily(tagFamily), m_tagPoses(), m_tagSize(1.0), m_td(NULL), + m_tf(NULL) { switch (m_tagFamily) { case TAG_36h11: @@ -132,10 +131,8 @@ class vpDetectorAprilTag::Impl } } - bool detect(const vpImage &I, - std::vector > &polygons, - std::vector &messages, const bool computePose, - const bool displayTag) + bool detect(const vpImage &I, std::vector > &polygons, + std::vector &messages, const bool computePose, const bool displayTag) { m_tagPoses.clear(); @@ -165,29 +162,23 @@ class vpDetectorAprilTag::Impl messages[i] = ss.str(); if (displayTag) { - vpDisplay::displayLine(I, (int)det->p[0][1], (int)det->p[0][0], - (int)det->p[1][1], (int)det->p[1][0], + vpDisplay::displayLine(I, (int)det->p[0][1], (int)det->p[0][0], (int)det->p[1][1], (int)det->p[1][0], vpColor::red, 2); - vpDisplay::displayLine(I, (int)det->p[0][1], (int)det->p[0][0], - (int)det->p[3][1], (int)det->p[3][0], + vpDisplay::displayLine(I, (int)det->p[0][1], (int)det->p[0][0], (int)det->p[3][1], (int)det->p[3][0], vpColor::green, 2); - vpDisplay::displayLine(I, (int)det->p[1][1], (int)det->p[1][0], - (int)det->p[2][1], (int)det->p[2][0], + vpDisplay::displayLine(I, (int)det->p[1][1], (int)det->p[1][0], (int)det->p[2][1], (int)det->p[2][0], vpColor::blue, 2); - vpDisplay::displayLine(I, (int)det->p[2][1], (int)det->p[2][0], - (int)det->p[3][1], (int)det->p[3][0], + vpDisplay::displayLine(I, (int)det->p[2][1], (int)det->p[2][0], (int)det->p[3][1], (int)det->p[3][0], vpColor::yellow, 2); } if (computePose) { vpHomogeneousMatrix cMo; - if (m_poseEstimationMethod == HOMOGRAPHY_VIRTUAL_VS || - m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS) { + if (m_poseEstimationMethod == HOMOGRAPHY_VIRTUAL_VS || m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS) { double fx = m_cam.get_px(), fy = m_cam.get_py(); double cx = m_cam.get_u0(), cy = m_cam.get_v0(); - matd_t *M = - homography_to_pose(det->H, fx, fy, cx, cy, m_tagSize / 2); + matd_t *M = homography_to_pose(det->H, fx, fy, cx, cy, m_tagSize / 2); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { @@ -237,8 +228,7 @@ class vpDetectorAprilTag::Impl if (m_poseEstimationMethod != HOMOGRAPHY_VIRTUAL_VS) { if (m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS) { - vpHomogeneousMatrix cMo_dementhon, cMo_lagrange, - cMo_homography = cMo; + vpHomogeneousMatrix cMo_dementhon, cMo_lagrange, cMo_homography = cMo; double residual_dementhon = std::numeric_limits::max(), residual_lagrange = std::numeric_limits::max(); @@ -264,8 +254,7 @@ class vpDetectorAprilTag::Impl // cMo = cMo_homography; //already the case } } else { - pose.computePose( - m_mapOfCorrespondingPoseMethods[m_poseEstimationMethod], cMo); + pose.computePose(m_mapOfCorrespondingPoseMethods[m_poseEstimationMethod], cMo); } } @@ -280,48 +269,29 @@ class vpDetectorAprilTag::Impl return detected; } - void getTagPoses(std::vector &tagPoses) const - { - tagPoses = m_tagPoses; - } + void getTagPoses(std::vector &tagPoses) const { tagPoses = m_tagPoses; } void setCameraParameters(const vpCameraParameters &cam) { m_cam = cam; } void setNbThreads(const int nThreads) { m_td->nthreads = nThreads; } - void setQuadDecimate(const float quadDecimate) - { - m_td->quad_decimate = quadDecimate; - } + void setQuadDecimate(const float quadDecimate) { m_td->quad_decimate = quadDecimate; } void setQuadSigma(const float quadSigma) { m_td->quad_sigma = quadSigma; } - void setRefineDecode(const bool refineDecode) - { - m_td->refine_decode = refineDecode ? 1 : 0; - } + void setRefineDecode(const bool refineDecode) { m_td->refine_decode = refineDecode ? 1 : 0; } - void setRefineEdges(const bool refineEdges) - { - m_td->refine_edges = refineEdges ? 1 : 0; - } + void setRefineEdges(const bool refineEdges) { m_td->refine_edges = refineEdges ? 1 : 0; } - void setRefinePose(const bool refinePose) - { - m_td->refine_pose = refinePose ? 1 : 0; - } + void setRefinePose(const bool refinePose) { m_td->refine_pose = refinePose ? 1 : 0; } void setTagSize(const double tagSize) { m_tagSize = tagSize; } - void setPoseEstimationMethod(const vpPoseEstimationMethod &method) - { - m_poseEstimationMethod = method; - } + void setPoseEstimationMethod(const vpPoseEstimationMethod &method) { m_poseEstimationMethod = method; } protected: vpCameraParameters m_cam; - std::map - m_mapOfCorrespondingPoseMethods; + std::map m_mapOfCorrespondingPoseMethods; vpPoseEstimationMethod m_poseEstimationMethod; vpAprilTagFamily m_tagFamily; std::vector m_tagPoses; @@ -334,11 +304,10 @@ class vpDetectorAprilTag::Impl /*! Default constructor. */ -vpDetectorAprilTag::vpDetectorAprilTag( - const vpAprilTagFamily &tagFamily, - const vpPoseEstimationMethod &poseEstimationMethod) - : m_displayTag(false), m_poseEstimationMethod(poseEstimationMethod), - m_tagFamily(tagFamily), m_impl(new Impl(tagFamily, poseEstimationMethod)) +vpDetectorAprilTag::vpDetectorAprilTag(const vpAprilTagFamily &tagFamily, + const vpPoseEstimationMethod &poseEstimationMethod) + : m_displayTag(false), m_poseEstimationMethod(poseEstimationMethod), m_tagFamily(tagFamily), + m_impl(new Impl(tagFamily, poseEstimationMethod)) { } @@ -360,8 +329,7 @@ bool vpDetectorAprilTag::detect(const vpImage &I) m_polygon.clear(); m_nb_objects = 0; - bool detected = - m_impl->detect(I, m_polygon, m_message, false, m_displayTag); + bool detected = m_impl->detect(I, m_polygon, m_message, false, m_displayTag); m_nb_objects = m_message.size(); return detected; @@ -375,9 +343,7 @@ bool vpDetectorAprilTag::detect(const vpImage &I) the pattern. \param cam : Camera intrinsic parameters. \param cMo_vec : List of tag poses. \return true if at least one tag is detected. */ -bool vpDetectorAprilTag::detect(const vpImage &I, - const double tagSize, - const vpCameraParameters &cam, +bool vpDetectorAprilTag::detect(const vpImage &I, const double tagSize, const vpCameraParameters &cam, std::vector &cMo_vec) { m_message.clear(); @@ -409,8 +375,7 @@ void vpDetectorAprilTag::setAprilTagNbThreads(const int nThreads) \param poseEstimationMethod : The method to used. */ -void vpDetectorAprilTag::setAprilTagPoseEstimationMethod( - const vpPoseEstimationMethod &poseEstimationMethod) +void vpDetectorAprilTag::setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod) { m_poseEstimationMethod = poseEstimationMethod; m_impl->setPoseEstimationMethod(poseEstimationMethod); @@ -428,10 +393,7 @@ void vpDetectorAprilTag::setAprilTagPoseEstimationMethod( \param quadDecimate : Value for quad_decimate. */ -void vpDetectorAprilTag::setAprilTagQuadDecimate(const float quadDecimate) -{ - m_impl->setQuadDecimate(quadDecimate); -} +void vpDetectorAprilTag::setAprilTagQuadDecimate(const float quadDecimate) { m_impl->setQuadDecimate(quadDecimate); } /*! From the AprilTag code: @@ -445,10 +407,7 @@ void vpDetectorAprilTag::setAprilTagQuadDecimate(const float quadDecimate) \param quadSigma : Value for quad_sigma. */ -void vpDetectorAprilTag::setAprilTagQuadSigma(const float quadSigma) -{ - m_impl->setQuadSigma(quadSigma); -} +void vpDetectorAprilTag::setAprilTagQuadSigma(const float quadSigma) { m_impl->setQuadSigma(quadSigma); } /*! From the AprilTag code: @@ -462,10 +421,7 @@ void vpDetectorAprilTag::setAprilTagQuadSigma(const float quadSigma) \param refineDecode : If true, set refine_decode to 1. */ -void vpDetectorAprilTag::setAprilTagRefineDecode(const bool refineDecode) -{ - m_impl->setRefineDecode(refineDecode); -} +void vpDetectorAprilTag::setAprilTagRefineDecode(const bool refineDecode) { m_impl->setRefineDecode(refineDecode); } /*! From the AprilTag code: @@ -481,10 +437,7 @@ void vpDetectorAprilTag::setAprilTagRefineDecode(const bool refineDecode) \param refineEdges : If true, set refine_edges to 1. */ -void vpDetectorAprilTag::setAprilTagRefineEdges(const bool refineEdges) -{ - m_impl->setRefineEdges(refineEdges); -} +void vpDetectorAprilTag::setAprilTagRefineEdges(const bool refineEdges) { m_impl->setRefineEdges(refineEdges); } /*! From the AprilTag code: @@ -502,10 +455,7 @@ void vpDetectorAprilTag::setAprilTagRefineEdges(const bool refineEdges) \param refinePose : If true, set refine_pose to 1. */ -void vpDetectorAprilTag::setAprilTagRefinePose(const bool refinePose) -{ - m_impl->setRefinePose(refinePose); -} +void vpDetectorAprilTag::setAprilTagRefinePose(const bool refinePose) { m_impl->setRefinePose(refinePose); } #elif !defined(VISP_BUILD_SHARED_LIBS) // Work arround to avoid warning: libvisp_core.a(vpDetectorAprilTag.cpp.o) has // no symbols diff --git a/modules/detection/src/vpDetectorBase.cpp b/modules/detection/src/vpDetectorBase.cpp index f8347dae8d..8f9e6adff4 100644 --- a/modules/detection/src/vpDetectorBase.cpp +++ b/modules/detection/src/vpDetectorBase.cpp @@ -42,9 +42,7 @@ /*! Default constructor. */ -vpDetectorBase::vpDetectorBase() : m_polygon(), m_message(), m_nb_objects(0) -{ -} +vpDetectorBase::vpDetectorBase() : m_polygon(), m_message(), m_nb_objects(0) {} /*! Returns ith object container box as a vector of points. @@ -54,10 +52,8 @@ std::vector &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())); + throw(vpException(vpException::badValue, "Bad index to retrieve object %d. Only %d objects are detected.", i, + m_polygon.size())); } /*! @@ -68,10 +64,8 @@ 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())); + throw(vpException(vpException::badValue, "Bad index to retrieve object %d . Only %d objects are detected.", i, + m_polygon.size())); } /*! diff --git a/modules/detection/test/testAprilTag.cpp b/modules/detection/test/testAprilTag.cpp index e2785d3739..ceb66da7e2 100644 --- a/modules/detection/test/testAprilTag.cpp +++ b/modules/detection/test/testAprilTag.cpp @@ -111,8 +111,7 @@ void usage(const char *name, const char *badparam, std::string ipath) \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, - std::string &ppath, bool &click_allowed, bool &display) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -158,10 +157,7 @@ struct TagGroundTruth { std::string message; std::vector corners; - TagGroundTruth(const std::string &msg, const std::vector &c) - : message(msg), corners(c) - { - } + TagGroundTruth(const std::string &msg, const std::vector &c) : message(msg), corners(c) {} bool operator==(const TagGroundTruth &b) const { @@ -212,8 +208,7 @@ int main(int argc, const char *argv[]) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed, - opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed, opt_display) == false) { exit(EXIT_FAILURE); } @@ -227,8 +222,7 @@ int main(int argc, const char *argv[]) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -245,8 +239,7 @@ int main(int argc, const char *argv[]) } if (!vpIoTools::checkFilename(filename)) { - std::cerr << "Filename: " << filename << " does not exist." - << std::endl; + std::cerr << "Filename: " << filename << " does not exist." << std::endl; return EXIT_SUCCESS; } vpImageIo::read(I, filename); @@ -261,46 +254,37 @@ int main(int argc, const char *argv[]) opt_display = false; #endif - vpDetectorAprilTag::vpAprilTagFamily tagFamily = - vpDetectorAprilTag::TAG_36h11; - vpDetectorAprilTag::vpPoseEstimationMethod poseEstimationMethod = - vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS; + vpDetectorAprilTag::vpAprilTagFamily tagFamily = vpDetectorAprilTag::TAG_36h11; + vpDetectorAprilTag::vpPoseEstimationMethod poseEstimationMethod = vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS; double tagSize = 0.053; float quad_decimate = 1.0; int nThreads = 1; bool display_tag = true; vpDetectorBase *detector = new vpDetectorAprilTag(tagFamily); - dynamic_cast(detector)->setAprilTagQuadDecimate( - quad_decimate); - dynamic_cast(detector) - ->setAprilTagPoseEstimationMethod(poseEstimationMethod); - dynamic_cast(detector)->setAprilTagNbThreads( - nThreads); + dynamic_cast(detector)->setAprilTagQuadDecimate(quad_decimate); + dynamic_cast(detector)->setAprilTagPoseEstimationMethod(poseEstimationMethod); + dynamic_cast(detector)->setAprilTagNbThreads(nThreads); dynamic_cast(detector)->setDisplayTag(display_tag); vpCameraParameters cam; - cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, - 243.4373779); + cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779); if (opt_display) { -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_OPENCV) +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) d.init(I, 0, 0, "AprilTag detection"); #endif vpDisplay::display(I); } std::vector cMo_vec; - dynamic_cast(detector)->detect(I, tagSize, cam, - cMo_vec); + dynamic_cast(detector)->detect(I, tagSize, cam, cMo_vec); // Ground truth std::map mapOfTagsGroundTruth; bool use_detection_ground_truth = false; { - std::string filename_ground_truth = vpIoTools::createFilePath( - ipath, "AprilTag/ground_truth_detection.txt"); + std::string filename_ground_truth = vpIoTools::createFilePath(ipath, "AprilTag/ground_truth_detection.txt"); std::ifstream file_ground_truth(filename_ground_truth.c_str()); if (file_ground_truth.is_open() && opt_ppath.empty()) { use_detection_ground_truth = true; @@ -308,15 +292,13 @@ int main(int argc, const char *argv[]) std::string message = ""; double v1 = 0.0, v2 = 0.0, v3 = 0.0, v4 = 0.0; double u1 = 0.0, u2 = 0.0, u3 = 0.0, u4 = 0.0; - while (file_ground_truth >> message >> v1 >> u1 >> v2 >> u2 >> v3 >> - u3 >> v4 >> u4) { + while (file_ground_truth >> message >> v1 >> u1 >> v2 >> u2 >> v3 >> u3 >> v4 >> u4) { std::vector tagCorners(4); tagCorners[0].set_ij(v1, u1); tagCorners[1].set_ij(v2, u2); tagCorners[2].set_ij(v3, u3); tagCorners[3].set_ij(v4, u4); - mapOfTagsGroundTruth.insert( - std::make_pair(message, TagGroundTruth(message, tagCorners))); + mapOfTagsGroundTruth.insert(std::make_pair(message, TagGroundTruth(message, tagCorners))); } } } @@ -324,8 +306,7 @@ int main(int argc, const char *argv[]) std::map mapOfPosesGroundTruth; bool use_pose_ground_truth = false; { - std::string filename_ground_truth = - vpIoTools::createFilePath(ipath, "AprilTag/ground_truth_pose.txt"); + std::string filename_ground_truth = vpIoTools::createFilePath(ipath, "AprilTag/ground_truth_pose.txt"); std::ifstream file_ground_truth(filename_ground_truth.c_str()); if (file_ground_truth.is_open() && opt_ppath.empty()) { use_pose_ground_truth = true; @@ -333,10 +314,8 @@ int main(int argc, const char *argv[]) std::string message = ""; double tx = 0.0, ty = 0.0, tz = 0.0; double tux = 0.0, tuy = 0.0, tuz = 0.0; - while (file_ground_truth >> message >> tx >> ty >> tz >> tux >> tuy >> - tuz) { - mapOfPosesGroundTruth.insert(std::make_pair( - message, vpPoseVector(tx, ty, tz, tux, tuy, tuz))); + while (file_ground_truth >> message >> tx >> ty >> tz >> tux >> tuy >> tuz) { + mapOfPosesGroundTruth.insert(std::make_pair(message, vpPoseVector(tx, ty, tz, tux, tuy, tuz))); } } } @@ -347,17 +326,13 @@ int main(int argc, const char *argv[]) if (use_detection_ground_truth) { std::string message = detector->getMessage(i); std::replace(message.begin(), message.end(), ' ', '_'); - std::map::iterator it = - mapOfTagsGroundTruth.find(message); + std::map::iterator it = mapOfTagsGroundTruth.find(message); TagGroundTruth current(message, p); if (it == mapOfTagsGroundTruth.end()) { - std::cerr << "Problem with tag decoding (tag_family or id): " - << message << std::endl; + std::cerr << "Problem with tag decoding (tag_family or id): " << message << std::endl; return EXIT_FAILURE; } else if (it->second != current) { - std::cerr << "Problem, current detection:\n" - << current << "\nGround truth:\n" - << it->second << std::endl; + std::cerr << "Problem, current detection:\n" << current << "\nGround truth:\n" << it->second << std::endl; return EXIT_FAILURE; } } @@ -365,15 +340,13 @@ int main(int argc, const char *argv[]) if (opt_display) { vpRect bbox = detector->getBBox(i); vpDisplay::displayRectangle(I, bbox, vpColor::green); - vpDisplay::displayText(I, (int)(bbox.getTop() - 10), - (int)bbox.getLeft(), detector->getMessage(i), + vpDisplay::displayText(I, (int)(bbox.getTop() - 10), (int)bbox.getLeft(), detector->getMessage(i), vpColor::red); } } if (opt_display) { - vpDisplay::displayText(I, 20, 20, "Click to display tag poses", - vpColor::red); + vpDisplay::displayText(I, 20, 20, "Click to display tag poses", vpColor::red); vpDisplay::flush(I); if (opt_click_allowed) vpDisplay::getClick(I); @@ -383,25 +356,21 @@ int main(int argc, const char *argv[]) for (size_t i = 0; i < cMo_vec.size(); i++) { if (opt_display) - vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, - vpColor::none, 3); + vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3); if (use_pose_ground_truth) { vpPoseVector pose_vec(cMo_vec[i]); std::string message = detector->getMessage(i); std::replace(message.begin(), message.end(), ' ', '_'); - std::map::iterator it = - mapOfPosesGroundTruth.find(message); + std::map::iterator it = mapOfPosesGroundTruth.find(message); if (it == mapOfPosesGroundTruth.end()) { - std::cerr << "Problem with tag decoding (tag_family or id): " - << message << std::endl; + std::cerr << "Problem with tag decoding (tag_family or id): " << message << std::endl; return EXIT_FAILURE; } else { for (unsigned int cpt = 0; cpt < 6; cpt++) { if (!vpMath::equal(it->second[cpt], pose_vec[cpt], 0.005)) { - std::cerr << "Problem, current pose: " << pose_vec.t() - << "\nGround truth pose: " << it->second.t() + std::cerr << "Problem, current pose: " << pose_vec.t() << "\nGround truth pose: " << it->second.t() << std::endl; return EXIT_FAILURE; } diff --git a/modules/gui/include/visp3/gui/vpD3DRenderer.h b/modules/gui/include/visp3/gui/vpD3DRenderer.h index 88c363e148..475b1910cf 100644 --- a/modules/gui/include/visp3/gui/vpD3DRenderer.h +++ b/modules/gui/include/visp3/gui/vpD3DRenderer.h @@ -105,35 +105,29 @@ class VISP_EXPORT vpD3DRenderer : public vpWin32Renderer void setImg(const vpImage &im); void setImg(const vpImage &im); - void setImgROI(const vpImage &im, const vpImagePoint &iP, - const unsigned int width, const unsigned int height); - void setImgROI(const vpImage &im, const vpImagePoint &iP, - const unsigned int width, const unsigned int height); + void setImgROI(const vpImage &im, const vpImagePoint &iP, const unsigned int width, + const unsigned int height); + void setImgROI(const vpImage &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, + 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 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, + 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 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 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, + void drawArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int w, unsigned int h, unsigned int thickness = 1); void getImage(vpImage &I); @@ -145,20 +139,15 @@ class VISP_EXPORT vpD3DRenderer : public vpWin32Renderer 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); - - void convert(const vpImage &I, unsigned char *imBuffer, - unsigned int pitch); - void convert(const vpImage &I, unsigned char *imBuffer, - unsigned int pitch); - void convertROI(const vpImage &I, unsigned char *imBuffer, - unsigned int pitch, int i_min, int j_min, int i_max, - int j_max); - void convertROI(const vpImage &I, unsigned char *imBuffer, - unsigned int pitch, int i_min, int j_min, int i_max, - int j_max); + void subDrawCircle(int i, int j, int x, int y, vpColor col, unsigned char *buf, unsigned int pitch, unsigned int maxX, + unsigned int maxY); + + void convert(const vpImage &I, unsigned char *imBuffer, unsigned int pitch); + void convert(const vpImage &I, unsigned char *imBuffer, unsigned int pitch); + void convertROI(const vpImage &I, unsigned char *imBuffer, unsigned int pitch, int i_min, int j_min, + int i_max, int j_max); + void convertROI(const vpImage &I, unsigned char *imBuffer, unsigned int pitch, int i_min, int j_min, + int i_max, int j_max); /*! Safe useful inline function to set a pixel in a texture buffer. @@ -172,9 +161,8 @@ class VISP_EXPORT vpD3DRenderer : public vpWin32Renderer locked rectangle). */ - inline void setBufferPixel(unsigned char *buf, unsigned int pitch, int x, - int y, const vpColor &color, unsigned int maxX, - unsigned int maxY) + 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) @@ -195,8 +183,7 @@ class VISP_EXPORT vpD3DRenderer : public vpWin32Renderer \param color The color of the pixel. */ - inline void setBufferPixel(unsigned char *buf, unsigned int pitch, int x, - int y, const vpColor &color) + 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) diff --git a/modules/gui/include/visp3/gui/vpDisplayD3D.h b/modules/gui/include/visp3/gui/vpDisplayD3D.h index 1823aee178..4d37777ab9 100644 --- a/modules/gui/include/visp3/gui/vpDisplayD3D.h +++ b/modules/gui/include/visp3/gui/vpDisplayD3D.h @@ -110,12 +110,10 @@ class VISP_EXPORT vpDisplayD3D : public vpDisplayWin32 vpDisplayD3D(); vpDisplayD3D(int winx, int winy, const std::string &title = ""); vpDisplayD3D(vpImage &I, vpScaleType type); - vpDisplayD3D(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = "", + vpDisplayD3D(vpImage &I, int winx = -1, int winy = -1, const std::string &title = "", vpScaleType type = SCALE_DEFAULT); vpDisplayD3D(vpImage &I, vpScaleType type); - vpDisplayD3D(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = "", + vpDisplayD3D(vpImage &I, int winx = -1, int winy = -1, const std::string &title = "", vpScaleType type = SCALE_DEFAULT); virtual ~vpDisplayD3D(); diff --git a/modules/gui/include/visp3/gui/vpDisplayGDI.h b/modules/gui/include/visp3/gui/vpDisplayGDI.h index 42058ca6dc..a469d190d5 100644 --- a/modules/gui/include/visp3/gui/vpDisplayGDI.h +++ b/modules/gui/include/visp3/gui/vpDisplayGDI.h @@ -96,8 +96,7 @@ int main() vpImagePoint topLeftCorner; topLeftCorner.set_i(50); topLeftCorner.set_j(10); - vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, -true); + vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true); // Flush the foreground and background display vpDisplay::flush(I); @@ -109,8 +108,8 @@ true); 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); + std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl; + vpTime::wait(40); } // Get a blocking keyboard event @@ -133,12 +132,10 @@ class VISP_EXPORT vpDisplayGDI : public vpDisplayWin32 vpDisplayGDI(); vpDisplayGDI(int winx, int winy, const std::string &title = ""); vpDisplayGDI(vpImage &I, vpScaleType type); - vpDisplayGDI(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = "", + vpDisplayGDI(vpImage &I, int winx = -1, int winy = -1, const std::string &title = "", vpScaleType type = SCALE_DEFAULT); vpDisplayGDI(vpImage &I, vpScaleType type); - vpDisplayGDI(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = "", + vpDisplayGDI(vpImage &I, int winx = -1, int winy = -1, const std::string &title = "", vpScaleType type = SCALE_DEFAULT); virtual ~vpDisplayGDI(); diff --git a/modules/gui/include/visp3/gui/vpDisplayGTK.h b/modules/gui/include/visp3/gui/vpDisplayGTK.h index e10f325048..d7bec9b69a 100644 --- a/modules/gui/include/visp3/gui/vpDisplayGTK.h +++ b/modules/gui/include/visp3/gui/vpDisplayGTK.h @@ -105,8 +105,7 @@ int main() vpImagePoint topLeftCorner; topLeftCorner.set_i(50); topLeftCorner.set_j(10); - vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, -true); + vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true); // Flush the foreground and background display vpDisplay::flush(I); @@ -118,8 +117,8 @@ true); 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); + std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl; + vpTime::wait(40); } // Get a blocking keyboard event @@ -143,9 +142,8 @@ class VISP_EXPORT vpDisplayGTK : public vpDisplay GtkWidget *widget; GdkPixmap *m_background; GdkGC *m_gc; - GdkColor blue, red, yellow, green, cyan, orange, white, black, gdkcolor, - lightBlue, darkBlue, lightRed, darkRed, lightGreen, darkGreen, purple, - lightGray, gray, darkGray; + GdkColor blue, red, yellow, green, cyan, orange, white, black, gdkcolor, lightBlue, darkBlue, lightRed, darkRed, + lightGreen, darkGreen, purple, lightGray, gray, darkGray; GdkColormap *colormap; GdkFont *font; @@ -179,12 +177,10 @@ class VISP_EXPORT vpDisplayGTK : public vpDisplay vpDisplayGTK(); vpDisplayGTK(int winx, int winy, const std::string &title = ""); vpDisplayGTK(vpImage &I, vpScaleType type); - vpDisplayGTK(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = "", + vpDisplayGTK(vpImage &I, int winx = -1, int winy = -1, const std::string &title = "", vpScaleType type = SCALE_DEFAULT); vpDisplayGTK(vpImage &I, vpScaleType type); - vpDisplayGTK(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = "", + vpDisplayGTK(vpImage &I, int winx = -1, int winy = -1, const std::string &title = "", vpScaleType type = SCALE_DEFAULT); virtual ~vpDisplayGTK(); @@ -195,12 +191,9 @@ class VISP_EXPORT vpDisplayGTK : public vpDisplay void getScreenSize(unsigned int &width, unsigned int &height); unsigned int getScreenWidth(); - void init(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = ""); - void init(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = ""); - void init(unsigned int width, unsigned int height, int winx = -1, - int winy = -1, const std::string &title = ""); + void init(vpImage &I, int winx = -1, int winy = -1, const std::string &title = ""); + void init(vpImage &I, int winx = -1, int winy = -1, const std::string &title = ""); + void init(unsigned int width, unsigned int height, int winx = -1, int winy = -1, const std::string &title = ""); protected: void setFont(const std::string &fontname); @@ -211,54 +204,41 @@ class VISP_EXPORT vpDisplayGTK : public vpDisplay 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 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, + 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 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 &I); void displayImage(const vpImage &I); void displayImage(const unsigned char *I); - void displayImageROI(const vpImage &I, - const vpImagePoint &iP, const unsigned int width, + void displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, + const unsigned int height); + void displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height); - void displayImageROI(const vpImage &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 displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness = 1); - void displayPoint(const vpImagePoint &ip, const vpColor &color, - unsigned int thickness = 1); - 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, + void displayPoint(const vpImagePoint &ip, const vpColor &color, unsigned int thickness = 1); + void displayRectangle(const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill = false, unsigned int thickness = 1); - void displayRectangle(const vpRect &rectangle, const vpColor &color, + 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); + 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 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(std::string &key, bool blocking = true); bool getPointerMotionEvent(vpImagePoint &ip); diff --git a/modules/gui/include/visp3/gui/vpDisplayOpenCV.h b/modules/gui/include/visp3/gui/vpDisplayOpenCV.h index dc61da4135..d31ce17236 100644 --- a/modules/gui/include/visp3/gui/vpDisplayOpenCV.h +++ b/modules/gui/include/visp3/gui/vpDisplayOpenCV.h @@ -108,8 +108,7 @@ int main() vpImagePoint topLeftCorner; topLeftCorner.set_i(10); topLeftCorner.set_j(50); - vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, -true); + vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true); // Flush the foreground and background display vpDisplay::flush(I); @@ -121,8 +120,8 @@ true); 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); + std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl; + vpTime::wait(40); } // Get a blocking keyboard event @@ -212,12 +211,10 @@ class VISP_EXPORT vpDisplayOpenCV : public vpDisplay vpDisplayOpenCV(); vpDisplayOpenCV(int winx, int winy, const std::string &title = ""); vpDisplayOpenCV(vpImage &I, vpScaleType type); - vpDisplayOpenCV(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = "", + vpDisplayOpenCV(vpImage &I, int winx = -1, int winy = -1, const std::string &title = "", vpScaleType type = SCALE_DEFAULT); vpDisplayOpenCV(vpImage &I, vpScaleType type); - vpDisplayOpenCV(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = "", + vpDisplayOpenCV(vpImage &I, int winx = -1, int winy = -1, const std::string &title = "", vpScaleType type = SCALE_DEFAULT); virtual ~vpDisplayOpenCV(); @@ -227,12 +224,9 @@ class VISP_EXPORT vpDisplayOpenCV : public vpDisplay void getScreenSize(unsigned int &width, unsigned int &height); unsigned int getScreenWidth(); - void init(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = ""); - void init(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = ""); - void init(unsigned int width, unsigned int height, int winx = -1, - int winy = -1, const std::string &title = ""); + void init(vpImage &I, int winx = -1, int winy = -1, const std::string &title = ""); + void init(vpImage &I, int winx = -1, int winy = -1, const std::string &title = ""); + void init(unsigned int width, unsigned int height, int winx = -1, int winy = -1, const std::string &title = ""); protected: void setFont(const std::string &font); @@ -243,55 +237,42 @@ class VISP_EXPORT vpDisplayOpenCV : public vpDisplay 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 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 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, + 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 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 &I); void displayImage(const vpImage &I); void displayImage(const unsigned char *I); - void displayImageROI(const vpImage &I, - const vpImagePoint &iP, const unsigned int width, + void displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, + const unsigned int height); + void displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height); - void displayImageROI(const vpImage &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, - 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, unsigned int thickness = 1); - 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, + void displayRectangle(const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill = false, unsigned int thickness = 1); - void displayRectangle(const vpRect &rectangle, const vpColor &color, + 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); + 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 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(std::string &key, bool blocking = true); diff --git a/modules/gui/include/visp3/gui/vpDisplayWin32.h b/modules/gui/include/visp3/gui/vpDisplayWin32.h index 764b582bc2..210f10d14f 100644 --- a/modules/gui/include/visp3/gui/vpDisplayWin32.h +++ b/modules/gui/include/visp3/gui/vpDisplayWin32.h @@ -120,11 +120,9 @@ class VISP_EXPORT vpDisplayWin32 : public vpDisplay public: explicit vpDisplayWin32(vpWin32Renderer *rend = NULL); - vpDisplayWin32(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = ""); + vpDisplayWin32(vpImage &I, int winx = -1, int winy = -1, const std::string &title = ""); - vpDisplayWin32(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = ""); + vpDisplayWin32(vpImage &I, int winx = -1, int winy = -1, const std::string &title = ""); virtual ~vpDisplayWin32(); @@ -133,27 +131,22 @@ class VISP_EXPORT vpDisplayWin32 : public vpDisplay void displayImage(const vpImage &I); void displayImage(const vpImage &I); - void displayImageROI(const vpImage &I, - const vpImagePoint &iP, const unsigned int width, + void displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, + const unsigned int height); + void displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height); - void displayImageROI(const vpImage &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 flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height); void getImage(vpImage &I); unsigned int getScreenHeight(); void getScreenSize(unsigned int &width, unsigned int &height); unsigned int getScreenWidth(); - void init(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = ""); - void init(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = ""); - void init(unsigned int width, unsigned int height, int winx = -1, - int winy = -1, const std::string &title = ""); + void init(vpImage &I, int winx = -1, int winy = -1, const std::string &title = ""); + void init(vpImage &I, int winx = -1, int winy = -1, const std::string &title = ""); + void init(unsigned int width, unsigned int height, int winx = -1, int winy = -1, const std::string &title = ""); void setFont(const std::string &fontname); void setDownScalingFactor(unsigned int scale) @@ -161,55 +154,41 @@ class VISP_EXPORT vpDisplayWin32 : public vpDisplay window.setScale(scale); m_scale = scale; } - void setDownScalingFactor(vpScaleType scaleType) - { - m_scaleType = scaleType; - } + void setDownScalingFactor(vpScaleType scaleType) { m_scaleType = scaleType; } void setTitle(const std::string &windowtitle); void setWindowPosition(int winx, int winy); 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 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 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, + 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 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 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 displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness = 1); - void displayPoint(const vpImagePoint &ip, const vpColor &color, - unsigned int thickness = 1); + void displayPoint(const vpImagePoint &ip, const vpColor &color, unsigned int thickness = 1); - 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, + void displayRectangle(const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill = false, unsigned int thickness = 1); - void displayRectangle(const vpRect &rectangle, const vpColor &color, + 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 getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true); - bool getClickUp(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(std::string &key, bool blocking); bool getPointerMotionEvent(vpImagePoint &ip); diff --git a/modules/gui/include/visp3/gui/vpDisplayX.h b/modules/gui/include/visp3/gui/vpDisplayX.h index 1db7d74acd..eb118e7e4c 100644 --- a/modules/gui/include/visp3/gui/vpDisplayX.h +++ b/modules/gui/include/visp3/gui/vpDisplayX.h @@ -117,8 +117,7 @@ int main() vpImagePoint topLeftCorner; topLeftCorner.set_i(50); topLeftCorner.set_j(10); - vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, -true); + vpDisplay::displayRectangle(I, topLeftCorner, 100, 20, vpColor::green, true); // Flush the foreground and background display vpDisplay::flush(I); @@ -130,8 +129,8 @@ true); 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); + std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl; + vpTime::wait(40); } // Get a blocking keyboard event @@ -191,11 +190,11 @@ class VISP_EXPORT vpDisplayX : public vpDisplay vpDisplayX(); vpDisplayX(int winx, int winy, const std::string &title = ""); vpDisplayX(vpImage &I, vpScaleType type); - vpDisplayX(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = "", vpScaleType type = SCALE_DEFAULT); + vpDisplayX(vpImage &I, int winx = -1, int winy = -1, const std::string &title = "", + vpScaleType type = SCALE_DEFAULT); vpDisplayX(vpImage &I, vpScaleType type); - vpDisplayX(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = "", vpScaleType type = SCALE_DEFAULT); + vpDisplayX(vpImage &I, int winx = -1, int winy = -1, const std::string &title = "", + vpScaleType type = SCALE_DEFAULT); virtual ~vpDisplayX(); @@ -205,67 +204,51 @@ class VISP_EXPORT vpDisplayX : public vpDisplay void getScreenSize(unsigned int &width, unsigned int &height); unsigned int getScreenWidth(); - void init(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = ""); - void init(vpImage &I, int winx = -1, int winy = -1, - const std::string &title = ""); - void init(unsigned int width, unsigned int height, int winx = -1, - int winy = -1, const std::string &title = ""); + void init(vpImage &I, int winx = -1, int winy = -1, const std::string &title = ""); + void init(vpImage &I, int winx = -1, int winy = -1, const std::string &title = ""); + void init(unsigned int width, unsigned int height, int winx = -1, int winy = -1, const std::string &title = ""); protected: 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 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 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, + 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 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 &I); void displayImage(const vpImage &I); void displayImage(const unsigned char *I); - void displayImageROI(const vpImage &I, - const vpImagePoint &iP, const unsigned int width, + void displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, + const unsigned int height); + void displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height); - void displayImageROI(const vpImage &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, - 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, unsigned int thickness = 1); - 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, + void displayRectangle(const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill = false, unsigned int thickness = 1); - void displayRectangle(const vpRect &rectangle, const vpColor &color, + 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); + 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 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(std::string &key, bool blocking = true); diff --git a/modules/gui/include/visp3/gui/vpGDIRenderer.h b/modules/gui/include/visp3/gui/vpGDIRenderer.h index 9ce861462b..923dd595cd 100644 --- a/modules/gui/include/visp3/gui/vpGDIRenderer.h +++ b/modules/gui/include/visp3/gui/vpGDIRenderer.h @@ -87,46 +87,37 @@ class VISP_EXPORT vpGDIRenderer : public vpWin32Renderer void setImg(const vpImage &I); void setImg(const vpImage &I); - void setImgROI(const vpImage &I, const vpImagePoint &iP, - const unsigned int width, const unsigned int height); - void setImgROI(const vpImage &I, const vpImagePoint &iP, - const unsigned int width, const unsigned int height); + void setImgROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height); + void setImgROI(const vpImage &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, + 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 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, + 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 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 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, + void drawArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int w, unsigned int h, unsigned int thickness = 1); void getImage(vpImage &I); private: // updates the renderer hbitmaps. - bool updateBitmap(HBITMAP &hBmp, unsigned char *imBuffer, unsigned int w, - unsigned int h); + bool updateBitmap(HBITMAP &hBmp, unsigned char *imBuffer, unsigned int w, unsigned int h); // updates the renderer hbitmaps. - bool updateBitmapROI(unsigned char *imBuffer, int i_min, int j_min, int w, - int h); + bool updateBitmapROI(unsigned char *imBuffer, int i_min, int j_min, int w, int h); // converts a vpImage into a HBITMAP . void convert(const vpImage &I, HBITMAP &hBmp); @@ -135,12 +126,12 @@ class VISP_EXPORT vpGDIRenderer : public vpWin32Renderer void convert(const vpImage &I, HBITMAP &hBmp); // converts a vpImage into a HBITMAP . - void convertROI(const vpImage &I, const vpImagePoint &iP, - const unsigned int width, const unsigned int height); + void convertROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, + const unsigned int height); // converst a vpImage into a HBITMAP . - void convertROI(const vpImage &I, const vpImagePoint &iP, - const unsigned int width, const unsigned int height); + void convertROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, + const unsigned int height); }; #endif #endif diff --git a/modules/gui/include/visp3/gui/vpPlot.h b/modules/gui/include/visp3/gui/vpPlot.h index 9031a37e61..4246209848 100644 --- a/modules/gui/include/visp3/gui/vpPlot.h +++ b/modules/gui/include/visp3/gui/vpPlot.h @@ -74,8 +74,8 @@ int main () // 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 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); @@ -147,55 +147,37 @@ class VISP_EXPORT vpPlot 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 std::string &title = ""); + vpPlot(const unsigned int nbGraph, const unsigned int height = 700, const unsigned int width = 700, const int x = -1, + const int y = -1, const std::string &title = ""); ~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 std::string &title = ""); + 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 std::string &title = ""); 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 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, + 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 std::string &dataFile, - const std::string &title_prefix = ""); - 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); + void resetPointList(const unsigned int graphNum, const unsigned int curveNum); + + void saveData(const unsigned int graphNum, const std::string &dataFile, const std::string &title_prefix = ""); + 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. @@ -207,14 +189,12 @@ class VISP_EXPORT vpPlot if (display->isInitialised()) vpDisplay::setFont(I, font.c_str()); } - void setLegend(const unsigned int graphNum, const unsigned int curveNum, - const std::string &legend); + void setLegend(const unsigned int graphNum, const unsigned int curveNum, const std::string &legend); void setTitle(const unsigned int graphNum, const std::string &title); void setUnitX(const unsigned int graphNum, const std::string &unitx); void setUnitY(const unsigned int graphNum, const std::string &unity); void setUnitZ(const unsigned int graphNum, const std::string &unitz); - void setThickness(const unsigned int graphNum, const unsigned int curveNum, - const unsigned int thickness); + void setThickness(const unsigned int graphNum, const unsigned int curveNum, const unsigned int thickness); private: void initNbGraph(unsigned int nbGraph); diff --git a/modules/gui/include/visp3/gui/vpPlotCurve.h b/modules/gui/include/visp3/gui/vpPlotCurve.h index fc5286857d..f114a7fa9a 100644 --- a/modules/gui/include/visp3/gui/vpPlotCurve.h +++ b/modules/gui/include/visp3/gui/vpPlotCurve.h @@ -79,10 +79,9 @@ class vpPlotCurve public: vpPlotCurve(); ~vpPlotCurve(); - void plotPoint(const vpImage &I, const vpImagePoint &iP, - const double x, const double y); - void plotList(const vpImage &I, const double xorg, - const double yorg, const double zoomx, const double zoomy); + void plotPoint(const vpImage &I, const vpImagePoint &iP, const double x, const double y); + void plotList(const vpImage &I, const double xorg, const double yorg, const double zoomx, + const double zoomy); }; #endif diff --git a/modules/gui/include/visp3/gui/vpPlotGraph.h b/modules/gui/include/visp3/gui/vpPlotGraph.h index 08fec39d85..da77c05573 100644 --- a/modules/gui/include/visp3/gui/vpPlotGraph.h +++ b/modules/gui/include/visp3/gui/vpPlotGraph.h @@ -178,17 +178,13 @@ class vpPlotGraph 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 &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 &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 initSize(vpImagePoint topLeft, unsigned int width, unsigned int height, unsigned int margei, + unsigned int margej); + void initScale(vpImage &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 &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 &I); void displayUnit(vpImage &I); @@ -200,18 +196,12 @@ class vpPlotGraph bool getPixelValue(vpImage &I, vpImagePoint &iP); - bool move(const vpImage &I, - vpMouseButton::vpMouseButtonType &button); - vpHomogeneousMatrix navigation(const vpImage &I, - bool &changed, - vpMouseButton::vpMouseButtonType &b); - - void plot(vpImage &I, const unsigned int curveNb, - const double x, const double y); - vpMouseButton::vpMouseButtonType plot(vpImage &I, - const unsigned int curveNb, - const double x, const double y, - const double z); + bool move(const vpImage &I, vpMouseButton::vpMouseButtonType &button); + vpHomogeneousMatrix navigation(const vpImage &I, bool &changed, vpMouseButton::vpMouseButtonType &b); + + void plot(vpImage &I, const unsigned int curveNb, const double x, const double y); + vpMouseButton::vpMouseButtonType plot(vpImage &I, const unsigned int curveNb, const double x, + const double y, const double z); void replot(vpImage &I); void replot3D(vpImage &I); @@ -223,12 +213,8 @@ class vpPlotGraph 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 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 std::string &legend); void setTitle(const std::string &title); void setUnitX(const std::string &unitx); diff --git a/modules/gui/include/visp3/gui/vpProjectionDisplay.h b/modules/gui/include/visp3/gui/vpProjectionDisplay.h index 08d4a9aa49..3e4b282b1c 100644 --- a/modules/gui/include/visp3/gui/vpProjectionDisplay.h +++ b/modules/gui/include/visp3/gui/vpProjectionDisplay.h @@ -115,15 +115,11 @@ class VISP_EXPORT vpProjectionDisplay } void insert(vpForwardProjection &fp); - void display(vpImage &I, const vpHomogeneousMatrix &cextMo, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &color, const bool &displayTraj = false, + void display(vpImage &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 &I, - const vpHomogeneousMatrix &cextMo, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const unsigned int thickness = 1); + void displayCamera(vpImage &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &cam, const unsigned int thickness = 1); private: std::list listFp; diff --git a/modules/gui/include/visp3/gui/vpWin32API.h b/modules/gui/include/visp3/gui/vpWin32API.h index 9bc3cfab77..0b895b3c55 100644 --- a/modules/gui/include/visp3/gui/vpWin32API.h +++ b/modules/gui/include/visp3/gui/vpWin32API.h @@ -51,21 +51,17 @@ 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 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 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 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); +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 index 385351e05e..61d752f80b 100644 --- a/modules/gui/include/visp3/gui/vpWin32Renderer.h +++ b/modules/gui/include/visp3/gui/vpWin32Renderer.h @@ -79,11 +79,9 @@ class VISP_EXPORT vpWin32Renderer */ virtual void setImg(const vpImage &im) = 0; virtual void setImg(const vpImage &im) = 0; - virtual void setImgROI(const vpImage &im, const vpImagePoint &iP, - const unsigned int width, + virtual void setImgROI(const vpImage &im, const vpImagePoint &iP, const unsigned int width, const unsigned int height) = 0; - virtual void setImgROI(const vpImage &im, - const vpImagePoint &iP, const unsigned int width, + virtual void setImgROI(const vpImage &im, const vpImagePoint &iP, const unsigned int width, const unsigned int height) = 0; /*! @@ -105,8 +103,7 @@ class VISP_EXPORT vpWin32Renderer \param thickness line thickness \param style style of the line */ - virtual void drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, - const vpColor &color, unsigned int thickness, + virtual void drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness, int style = PS_SOLID) = 0; /*! @@ -118,8 +115,7 @@ class VISP_EXPORT vpWin32Renderer \param fill True if it is a filled rectangle \param thickness line thickness */ - virtual void drawRect(const vpImagePoint &topLeft, unsigned int width, - unsigned int height, const vpColor &color, + virtual void drawRect(const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill = false, unsigned int thickness = 1) = 0; /*! @@ -136,8 +132,7 @@ class VISP_EXPORT vpWin32Renderer \param fill When true fill the circle with the given color \param thickness Drawing thickness */ - virtual void drawCircle(const vpImagePoint ¢er, unsigned int radius, - const vpColor &color, bool fill, + virtual void drawCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill, unsigned int thickness = 1) = 0; /*! @@ -146,8 +141,7 @@ class VISP_EXPORT vpWin32Renderer \param text The string to display \param color The text's color */ - virtual void drawText(const vpImagePoint &ip, const char *text, - const vpColor &color) = 0; + virtual void drawText(const vpImagePoint &ip, const char *text, const vpColor &color) = 0; /*! Draws a cross. @@ -156,8 +150,7 @@ class VISP_EXPORT vpWin32Renderer \param color The cross' color \param thickness Thickness of the drawing */ - virtual void drawCross(const vpImagePoint &ip, unsigned int size, - const vpColor &color, + virtual void drawCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness = 1) = 0; /*! @@ -168,9 +161,8 @@ class VISP_EXPORT vpWin32Renderer \param w,h Width and height of the arrow \param thickness Thickness of the drawing */ - virtual void drawArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, - const vpColor &color, unsigned int w, unsigned int h, - unsigned int thickness) = 0; + 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. diff --git a/modules/gui/include/visp3/gui/vpWin32Window.h b/modules/gui/include/visp3/gui/vpWin32Window.h index a5daa1b8d8..042293394b 100644 --- a/modules/gui/include/visp3/gui/vpWin32Window.h +++ b/modules/gui/include/visp3/gui/vpWin32Window.h @@ -121,8 +121,7 @@ class VISP_EXPORT vpWin32Window bool isInitialized() { return initialized; } //! Initialize the window - void initWindow(const char *title, int posx, int posy, unsigned int w, - unsigned int h); + void initWindow(const char *title, int posx, int posy, unsigned int w, unsigned int h); void setScale(unsigned int scale) { renderer->setScale(scale); }; @@ -132,8 +131,7 @@ class VISP_EXPORT vpWin32Window friend class vpDisplayGDI; //! The message loop - friend LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, - LPARAM lParam); + friend LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam); }; #endif diff --git a/modules/gui/src/display/vpDisplayGTK.cpp b/modules/gui/src/display/vpDisplayGTK.cpp index 3c2c62e6a9..ca49c90b0a 100644 --- a/modules/gui/src/display/vpDisplayGTK.cpp +++ b/modules/gui/src/display/vpDisplayGTK.cpp @@ -85,11 +85,9 @@ */ vpDisplayGTK::vpDisplayGTK(vpImage &I, vpScaleType scaleType) - : widget(NULL), m_background(NULL), m_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) + : widget(NULL), m_background(NULL), m_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) { setScale(scaleType, I.getWidth(), I.getHeight()); init(I); @@ -118,13 +116,10 @@ vpDisplayGTK::vpDisplayGTK(vpImage &I, vpScaleType scaleType) and the columns. */ -vpDisplayGTK::vpDisplayGTK(vpImage &I, int x, int y, - const std::string &title, vpScaleType scaleType) - : widget(NULL), m_background(NULL), m_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) +vpDisplayGTK::vpDisplayGTK(vpImage &I, int x, int y, const std::string &title, vpScaleType scaleType) + : widget(NULL), m_background(NULL), m_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) { setScale(scaleType, I.getWidth(), I.getHeight()); init(I, x, y, title); @@ -150,11 +145,9 @@ vpDisplayGTK::vpDisplayGTK(vpImage &I, int x, int y, and the columns. */ vpDisplayGTK::vpDisplayGTK(vpImage &I, vpScaleType scaleType) - : widget(NULL), m_background(NULL), m_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) + : widget(NULL), m_background(NULL), m_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) { setScale(scaleType, I.getWidth(), I.getHeight()); init(I); @@ -181,13 +174,10 @@ vpDisplayGTK::vpDisplayGTK(vpImage &I, vpScaleType scaleType) - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns. */ -vpDisplayGTK::vpDisplayGTK(vpImage &I, int x, int y, - const std::string &title, vpScaleType scaleType) - : widget(NULL), m_background(NULL), m_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) +vpDisplayGTK::vpDisplayGTK(vpImage &I, int x, int y, const std::string &title, vpScaleType scaleType) + : widget(NULL), m_background(NULL), m_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) { setScale(scaleType, I.getWidth(), I.getHeight()); init(I, x, y, title); @@ -216,11 +206,9 @@ int main() \endcode */ vpDisplayGTK::vpDisplayGTK(int x, int y, const std::string &title) - : widget(NULL), m_background(NULL), m_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) + : widget(NULL), m_background(NULL), m_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) { m_windowXPosition = x; m_windowYPosition = y; @@ -247,11 +235,9 @@ int main() \endcode */ vpDisplayGTK::vpDisplayGTK() - : vpDisplay(), widget(NULL), m_background(NULL), m_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) + : vpDisplay(), widget(NULL), m_background(NULL), m_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) { } @@ -269,12 +255,10 @@ vpDisplayGTK::~vpDisplayGTK() { closeDisplay(); } \param title : Window title. */ -void vpDisplayGTK::init(vpImage &I, int x, int y, - const std::string &title) +void vpDisplayGTK::init(vpImage &I, int x, int y, const std::string &title) { if ((I.getHeight() == 0) || (I.getWidth() == 0)) { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "Image not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized")); } if (x != -1) @@ -286,8 +270,7 @@ void vpDisplayGTK::init(vpImage &I, int x, int y, m_title = title; setScale(m_scaleType, I.getWidth(), I.getHeight()); - init(I.getWidth(), I.getHeight(), m_windowXPosition, m_windowYPosition, - m_title); + init(I.getWidth(), I.getHeight(), m_windowXPosition, m_windowYPosition, m_title); I.display = this; m_displayHasBeenInitialized = true; @@ -302,12 +285,10 @@ void vpDisplayGTK::init(vpImage &I, int x, int y, \param title : Window title. */ -void vpDisplayGTK::init(vpImage &I, int x, int y, - const std::string &title) +void vpDisplayGTK::init(vpImage &I, int x, int y, const std::string &title) { if ((I.getHeight() == 0) || (I.getWidth() == 0)) { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "Image not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized")); } if (x != -1) @@ -319,8 +300,7 @@ void vpDisplayGTK::init(vpImage &I, int x, int y, m_title = title; setScale(m_scaleType, I.getWidth(), I.getHeight()); - init(I.getWidth(), I.getHeight(), m_windowXPosition, m_windowYPosition, - m_title); + init(I.getWidth(), I.getHeight(), m_windowXPosition, m_windowYPosition, m_title); I.display = this; m_displayHasBeenInitialized = true; @@ -333,8 +313,7 @@ void vpDisplayGTK::init(vpImage &I, int x, int y, \param title : Window title. */ -void vpDisplayGTK::init(unsigned int w, unsigned int h, int x, int y, - const std::string &title) +void vpDisplayGTK::init(unsigned int w, unsigned int h, int x, int y, const std::string &title) { /* Initialisation of thegdk et gdk_rgb library */ int *argc = NULL; @@ -350,12 +329,9 @@ void vpDisplayGTK::init(unsigned int w, unsigned int h, int x, int y, /* 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_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)m_width, - (gint)m_height); + gtk_window_set_default_size(GTK_WINDOW(widget), (gint)m_width, (gint)m_height); if (x != -1) m_windowXPosition = x; @@ -369,8 +345,7 @@ void vpDisplayGTK::init(unsigned int w, unsigned int h, int x, int y, gdk_rgb_init(); /* Create background pixmap */ - m_background = - gdk_pixmap_new(widget->window, (gint)m_width, (gint)m_height, -1); + m_background = gdk_pixmap_new(widget->window, (gint)m_width, (gint)m_height, -1); /* Create graphic context */ m_gc = gdk_gc_new(widget->window); @@ -378,8 +353,7 @@ void vpDisplayGTK::init(unsigned int w, unsigned int h, int x, int y, /* get the colormap */ colormap = gdk_window_get_colormap(widget->window); - col = new GdkColor - *[vpColor::id_unknown]; // id_unknown = number of predefined colors + col = new GdkColor *[vpColor::id_unknown]; // id_unknown = number of predefined colors /* Create color */ gdk_color_parse("light blue", &lightBlue); @@ -459,8 +433,7 @@ void vpDisplayGTK::init(unsigned int w, unsigned int h, int x, int y, 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-*-*-*-*-*-*-*"); + font = gdk_font_load("-*-courier 10 pitch-medium-r-normal-*-16-*-*-*-*-*-*-*"); m_title = title; if (!title.empty()) @@ -483,10 +456,7 @@ void vpDisplayGTK::init(unsigned int w, unsigned int h, int x, int y, \sa displayCharString() */ -void vpDisplayGTK::setFont(const std::string &fontname) -{ - font = gdk_font_load((const gchar *)fontname.c_str()); -} +void vpDisplayGTK::setFont(const std::string &fontname) { font = gdk_font_load((const gchar *)fontname.c_str()); } /*! Set the window title. @@ -499,8 +469,7 @@ void vpDisplayGTK::setTitle(const std::string &title) if (!title.empty()) gdk_window_set_title(widget->window, m_title.c_str()); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -519,8 +488,7 @@ void vpDisplayGTK::setWindowPosition(int winx, int winy) if (m_displayHasBeenInitialized) { gtk_window_move(GTK_WINDOW(widget), winx, winy); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -541,22 +509,19 @@ void vpDisplayGTK::displayImage(const vpImage &I) if (m_displayHasBeenInitialized) { if (m_scale == 1) { /* Copie de l'image dans le pixmap fond */ - gdk_draw_gray_image(m_background, m_gc, 0, 0, (gint)m_width, - (gint)m_height, GDK_RGB_DITHER_NONE, I.bitmap, + gdk_draw_gray_image(m_background, m_gc, 0, 0, (gint)m_width, (gint)m_height, GDK_RGB_DITHER_NONE, I.bitmap, (gint)m_width); } else { vpImage sampled; I.subsample(m_scale, m_scale, sampled); - gdk_draw_gray_image(m_background, m_gc, 0, 0, (gint)m_width, - (gint)m_height, GDK_RGB_DITHER_NONE, sampled.bitmap, + gdk_draw_gray_image(m_background, m_gc, 0, 0, (gint)m_width, (gint)m_height, GDK_RGB_DITHER_NONE, sampled.bitmap, (gint)m_width); } /* Le pixmap background devient le fond de la zone de dessin */ gdk_window_set_back_pixmap(widget->window, m_background, FALSE); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -577,29 +542,24 @@ void vpDisplayGTK::displayImage(const vpImage &I) \sa init(), closeDisplay() */ -void vpDisplayGTK::displayImageROI(const vpImage &I, - const vpImagePoint &iP, - const unsigned int w, const unsigned int h) +void vpDisplayGTK::displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int w, + const unsigned int h) { if (m_displayHasBeenInitialized) { vpImage Itemp; - vpImageTools::crop(I, iP.get_i(), iP.get_j(), h, w, Itemp, m_scale, - m_scale); + vpImageTools::crop(I, iP.get_i(), iP.get_j(), h, w, Itemp, m_scale, m_scale); /* Copie de l'image dans le pixmap fond */ int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - gdk_draw_gray_image(m_background, m_gc, (gint)j_min, (gint)i_min, - (gint)Itemp.getWidth(), (gint)Itemp.getHeight(), - GDK_RGB_DITHER_NONE, Itemp.bitmap, - (gint)Itemp.getWidth()); + gdk_draw_gray_image(m_background, m_gc, (gint)j_min, (gint)i_min, (gint)Itemp.getWidth(), (gint)Itemp.getHeight(), + GDK_RGB_DITHER_NONE, Itemp.bitmap, (gint)Itemp.getWidth()); /* Le pixmap background devient le fond de la zone de dessin */ gdk_window_set_back_pixmap(widget->window, m_background, FALSE); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -620,16 +580,13 @@ void vpDisplayGTK::displayImage(const vpImage &I) if (m_displayHasBeenInitialized) { if (m_scale == 1) { /* Copie de l'image dans le pixmap fond */ - gdk_draw_rgb_32_image(m_background, m_gc, 0, 0, (gint)m_width, - (gint)m_height, GDK_RGB_DITHER_NONE, + gdk_draw_rgb_32_image(m_background, m_gc, 0, 0, (gint)m_width, (gint)m_height, GDK_RGB_DITHER_NONE, (unsigned char *)I.bitmap, (gint)(4 * m_width)); } else { vpImage sampled; I.subsample(m_scale, m_scale, sampled); - gdk_draw_rgb_32_image(m_background, m_gc, 0, 0, (gint)m_width, - (gint)m_height, GDK_RGB_DITHER_NONE, - (unsigned char *)sampled.bitmap, - (gint)(4 * m_width)); + gdk_draw_rgb_32_image(m_background, m_gc, 0, 0, (gint)m_width, (gint)m_height, GDK_RGB_DITHER_NONE, + (unsigned char *)sampled.bitmap, (gint)(4 * m_width)); } /* Permet de fermer la fenetre si besoin (cas des sequences d'images) */ // while (g_main_iteration(FALSE)); @@ -637,8 +594,7 @@ void vpDisplayGTK::displayImage(const vpImage &I) /* Le pixmap background devient le fond de la zone de dessin */ gdk_window_set_back_pixmap(widget->window, m_background, FALSE); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -659,29 +615,24 @@ void vpDisplayGTK::displayImage(const vpImage &I) \sa init(), closeDisplay() */ -void vpDisplayGTK::displayImageROI(const vpImage &I, - const vpImagePoint &iP, - const unsigned int w, const unsigned int h) +void vpDisplayGTK::displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int w, + const unsigned int h) { if (m_displayHasBeenInitialized) { vpImage Itemp; - vpImageTools::crop(I, iP.get_i(), iP.get_j(), h, w, Itemp, m_scale, - m_scale); + vpImageTools::crop(I, iP.get_i(), iP.get_j(), h, w, Itemp, m_scale, m_scale); /* Copie de l'image dans le pixmap fond */ int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - gdk_draw_rgb_32_image(m_background, m_gc, (gint)j_min, (gint)i_min, - (gint)Itemp.getWidth(), (gint)Itemp.getHeight(), - GDK_RGB_DITHER_NONE, (unsigned char *)Itemp.bitmap, - (gint)Itemp.getWidth() * 4); + gdk_draw_rgb_32_image(m_background, m_gc, (gint)j_min, (gint)i_min, (gint)Itemp.getWidth(), (gint)Itemp.getHeight(), + GDK_RGB_DITHER_NONE, (unsigned char *)Itemp.bitmap, (gint)Itemp.getWidth() * 4); /* Le pixmap background devient le fond de la zone de dessin */ gdk_window_set_back_pixmap(widget->window, m_background, FALSE); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -690,10 +641,7 @@ void vpDisplayGTK::displayImageROI(const vpImage &I, \sa init(), closeDisplay() */ -void vpDisplayGTK::displayImage(const unsigned char * /* I */) -{ - vpTRACE(" not implemented "); -} +void vpDisplayGTK::displayImage(const unsigned char * /* I */) { vpTRACE(" not implemented "); } /*! Close the window. @@ -726,8 +674,7 @@ void vpDisplayGTK::flushDisplay() gdk_window_clear(widget->window); gdk_flush(); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -735,26 +682,21 @@ void vpDisplayGTK::flushDisplay() 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*/, +void vpDisplayGTK::flushDisplayROI(const vpImagePoint & /*iP*/, const unsigned int /*width*/, const unsigned int /*height*/) { if (m_displayHasBeenInitialized) { gdk_window_clear(widget->window); gdk_flush(); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } /*! \warning Not implemented yet. */ -void vpDisplayGTK::clearDisplay(const vpColor & /* color */) -{ - vpTRACE("Not implemented"); -} +void vpDisplayGTK::clearDisplay(const vpColor & /* color */) { vpTRACE("Not implemented"); } /*! Display an arrow from image point \e ip1 to image point \e ip2. @@ -763,10 +705,8 @@ void vpDisplayGTK::clearDisplay(const vpColor & /* 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) +void vpDisplayGTK::displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int w, + unsigned int h, unsigned int thickness) { if (m_displayHasBeenInitialized) { double a = ip2.get_i() - ip1.get_i(); @@ -801,8 +741,7 @@ void vpDisplayGTK::displayArrow(const vpImagePoint &ip1, displayLine(ip1, ip2, color, thickness); } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -817,8 +756,7 @@ void vpDisplayGTK::displayArrow(const vpImagePoint &ip1, \sa setFont() */ -void vpDisplayGTK::displayCharString(const vpImagePoint &ip, const char *text, - const vpColor &color) +void vpDisplayGTK::displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color) { if (m_displayHasBeenInitialized) { if (color.id < vpColor::id_unknown) @@ -831,14 +769,12 @@ void vpDisplayGTK::displayCharString(const vpImagePoint &ip, const char *text, gdk_gc_set_foreground(m_gc, &gdkcolor); } if (font != NULL) - gdk_draw_string( - m_background, font, m_gc, vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale), (const gchar *)text); + gdk_draw_string(m_background, font, m_gc, vpMath::round(ip.get_u() / m_scale), + vpMath::round(ip.get_v() / m_scale), (const gchar *)text); else std::cout << "Cannot draw string: no font is selected" << std::endl; } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } /*! @@ -850,9 +786,8 @@ void vpDisplayGTK::displayCharString(const vpImagePoint &ip, const char *text, \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) +void vpDisplayGTK::displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill, + unsigned int thickness) { if (m_displayHasBeenInitialized) { if (thickness == 1) @@ -868,26 +803,18 @@ void vpDisplayGTK::displayCircle(const vpImagePoint ¢er, gdk_gc_set_foreground(m_gc, &gdkcolor); } - gdk_gc_set_line_attributes(m_gc, (gint)thickness, GDK_LINE_SOLID, - GDK_CAP_BUTT, GDK_JOIN_BEVEL); + gdk_gc_set_line_attributes(m_gc, (gint)thickness, GDK_LINE_SOLID, GDK_CAP_BUTT, GDK_JOIN_BEVEL); if (fill == false) - gdk_draw_arc(m_background, m_gc, FALSE, - vpMath::round((center.get_u() - radius) / m_scale), - vpMath::round((center.get_v() - radius) / m_scale), - (gint)(2. * radius / m_scale), - (gint)(2. * radius / m_scale), 23040, - 23040); /* 23040 = 360*64 */ + gdk_draw_arc(m_background, m_gc, FALSE, vpMath::round((center.get_u() - radius) / m_scale), + vpMath::round((center.get_v() - radius) / m_scale), (gint)(2. * radius / m_scale), + (gint)(2. * radius / m_scale), 23040, 23040); /* 23040 = 360*64 */ else - gdk_draw_arc(m_background, m_gc, TRUE, - vpMath::round((center.get_u() - radius) / m_scale), - vpMath::round((center.get_v() - radius) / m_scale), - (gint)(2. * radius / m_scale), - (gint)(2. * radius / m_scale), 23040, - 23040); /* 23040 = 360*64 */ + gdk_draw_arc(m_background, m_gc, TRUE, vpMath::round((center.get_u() - radius) / m_scale), + vpMath::round((center.get_v() - radius) / m_scale), (gint)(2. * radius / m_scale), + (gint)(2. * radius / m_scale), 23040, 23040); /* 23040 = 360*64 */ } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } /*! @@ -897,8 +824,7 @@ void vpDisplayGTK::displayCircle(const vpImagePoint ¢er, \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) +void vpDisplayGTK::displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness) { if (m_displayHasBeenInitialized) { double i = ip.get_i(); @@ -920,8 +846,7 @@ void vpDisplayGTK::displayCross(const vpImagePoint &ip, unsigned int size, } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } /*! @@ -930,9 +855,7 @@ void vpDisplayGTK::displayCross(const vpImagePoint &ip, unsigned int size, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplayGTK::displayDotLine(const vpImagePoint &ip1, - const vpImagePoint &ip2, - const vpColor &color, +void vpDisplayGTK::displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness) { @@ -950,17 +873,12 @@ void vpDisplayGTK::displayDotLine(const vpImagePoint &ip1, gdk_gc_set_foreground(m_gc, &gdkcolor); } - gdk_gc_set_line_attributes(m_gc, (gint)thickness, GDK_LINE_ON_OFF_DASH, - GDK_CAP_BUTT, GDK_JOIN_BEVEL); - gdk_draw_line(m_background, m_gc, vpMath::round(ip1.get_u() / m_scale), - vpMath::round(ip1.get_v() / m_scale), - vpMath::round(ip2.get_u() / m_scale), - vpMath::round(ip2.get_v() / m_scale)); - gdk_gc_set_line_attributes(m_gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT, - GDK_JOIN_BEVEL); + gdk_gc_set_line_attributes(m_gc, (gint)thickness, GDK_LINE_ON_OFF_DASH, GDK_CAP_BUTT, GDK_JOIN_BEVEL); + gdk_draw_line(m_background, m_gc, vpMath::round(ip1.get_u() / m_scale), vpMath::round(ip1.get_v() / m_scale), + vpMath::round(ip2.get_u() / m_scale), vpMath::round(ip2.get_v() / m_scale)); + gdk_gc_set_line_attributes(m_gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT, GDK_JOIN_BEVEL); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -970,8 +888,7 @@ void vpDisplayGTK::displayDotLine(const vpImagePoint &ip1, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplayGTK::displayLine(const vpImagePoint &ip1, - const vpImagePoint &ip2, const vpColor &color, +void vpDisplayGTK::displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness) { if (m_displayHasBeenInitialized) { @@ -988,15 +905,11 @@ void vpDisplayGTK::displayLine(const vpImagePoint &ip1, gdk_gc_set_foreground(m_gc, &gdkcolor); } - gdk_gc_set_line_attributes(m_gc, (gint)thickness, GDK_LINE_SOLID, - GDK_CAP_BUTT, GDK_JOIN_BEVEL); - gdk_draw_line(m_background, m_gc, vpMath::round(ip1.get_u() / m_scale), - vpMath::round(ip1.get_v() / m_scale), - vpMath::round(ip2.get_u() / m_scale), - vpMath::round(ip2.get_v() / m_scale)); + gdk_gc_set_line_attributes(m_gc, (gint)thickness, GDK_LINE_SOLID, GDK_CAP_BUTT, GDK_JOIN_BEVEL); + gdk_draw_line(m_background, m_gc, vpMath::round(ip1.get_u() / m_scale), vpMath::round(ip1.get_v() / m_scale), + vpMath::round(ip2.get_u() / m_scale), vpMath::round(ip2.get_v() / m_scale)); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -1006,8 +919,7 @@ void vpDisplayGTK::displayLine(const vpImagePoint &ip1, \param color : Point color. \param thickness : Point thickness. */ -void vpDisplayGTK::displayPoint(const vpImagePoint &ip, const vpColor &color, - unsigned int thickness) +void vpDisplayGTK::displayPoint(const vpImagePoint &ip, const vpColor &color, unsigned int thickness) { if (m_displayHasBeenInitialized) { if (color.id < vpColor::id_unknown) @@ -1021,16 +933,13 @@ void vpDisplayGTK::displayPoint(const vpImagePoint &ip, const vpColor &color, } if (thickness == 1) { - gdk_draw_point(m_background, m_gc, vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale)); + gdk_draw_point(m_background, m_gc, vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale)); } else { - gdk_draw_rectangle( - m_background, m_gc, TRUE, vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale), thickness, thickness); + gdk_draw_rectangle(m_background, m_gc, TRUE, vpMath::round(ip.get_u() / m_scale), + vpMath::round(ip.get_v() / m_scale), thickness, thickness); } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -1047,10 +956,8 @@ void vpDisplayGTK::displayPoint(const vpImagePoint &ip, const vpColor &color, 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) +void vpDisplayGTK::displayRectangle(const vpImagePoint &topLeft, unsigned int w, unsigned int h, const vpColor &color, + bool fill, unsigned int thickness) { if (m_displayHasBeenInitialized) { if (thickness == 1) @@ -1065,26 +972,19 @@ void vpDisplayGTK::displayRectangle(const vpImagePoint &topLeft, gdk_colormap_alloc_color(colormap, &gdkcolor, FALSE, TRUE); gdk_gc_set_foreground(m_gc, &gdkcolor); } - gdk_gc_set_line_attributes(m_gc, (gint)thickness, GDK_LINE_SOLID, - GDK_CAP_BUTT, GDK_JOIN_BEVEL); + gdk_gc_set_line_attributes(m_gc, (gint)thickness, GDK_LINE_SOLID, GDK_CAP_BUTT, GDK_JOIN_BEVEL); if (fill == false) - gdk_draw_rectangle(m_background, m_gc, FALSE, - vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale), - (gint)w / m_scale, (gint)h / m_scale); + gdk_draw_rectangle(m_background, m_gc, FALSE, vpMath::round(topLeft.get_u() / m_scale), + vpMath::round(topLeft.get_v() / m_scale), (gint)w / m_scale, (gint)h / m_scale); else - gdk_draw_rectangle(m_background, m_gc, TRUE, - vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale), - (gint)w / m_scale, (gint)h / m_scale); + gdk_draw_rectangle(m_background, m_gc, TRUE, vpMath::round(topLeft.get_u() / m_scale), + vpMath::round(topLeft.get_v() / m_scale), (gint)w / m_scale, (gint)h / m_scale); if (thickness > 1) - gdk_gc_set_line_attributes(m_gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT, - GDK_JOIN_BEVEL); + gdk_gc_set_line_attributes(m_gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT, GDK_JOIN_BEVEL); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -1100,10 +1000,8 @@ void vpDisplayGTK::displayRectangle(const vpImagePoint &topLeft, 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) +void vpDisplayGTK::displayRectangle(const vpImagePoint &topLeft, const vpImagePoint &bottomRight, const vpColor &color, + bool fill, unsigned int thickness) { if (m_displayHasBeenInitialized) { if (thickness == 1) @@ -1119,27 +1017,22 @@ void vpDisplayGTK::displayRectangle(const vpImagePoint &topLeft, gdk_gc_set_foreground(m_gc, &gdkcolor); } - gdk_gc_set_line_attributes(m_gc, (gint)thickness, GDK_LINE_SOLID, - GDK_CAP_BUTT, GDK_JOIN_BEVEL); + gdk_gc_set_line_attributes(m_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( - m_background, m_gc, FALSE, vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale), w / m_scale, h / m_scale); + gdk_draw_rectangle(m_background, m_gc, FALSE, vpMath::round(topLeft.get_u() / m_scale), + vpMath::round(topLeft.get_v() / m_scale), w / m_scale, h / m_scale); else - gdk_draw_rectangle( - m_background, m_gc, TRUE, vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale), w / m_scale, h / m_scale); + gdk_draw_rectangle(m_background, m_gc, TRUE, vpMath::round(topLeft.get_u() / m_scale), + vpMath::round(topLeft.get_v() / m_scale), w / m_scale, h / m_scale); if (thickness > 1) - gdk_gc_set_line_attributes(m_gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT, - GDK_JOIN_BEVEL); + gdk_gc_set_line_attributes(m_gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT, GDK_JOIN_BEVEL); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -1155,9 +1048,7 @@ void vpDisplayGTK::displayRectangle(const vpImagePoint &topLeft, false. */ -void vpDisplayGTK::displayRectangle(const vpRect &rectangle, - const vpColor &color, bool fill, - unsigned int thickness) +void vpDisplayGTK::displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill, unsigned int thickness) { if (m_displayHasBeenInitialized) { if (color.id < vpColor::id_unknown) @@ -1173,29 +1064,22 @@ void vpDisplayGTK::displayRectangle(const vpRect &rectangle, if (thickness == 1) thickness = 0; - gdk_gc_set_line_attributes(m_gc, (gint)thickness, GDK_LINE_SOLID, - GDK_CAP_BUTT, GDK_JOIN_BEVEL); + gdk_gc_set_line_attributes(m_gc, (gint)thickness, GDK_LINE_SOLID, GDK_CAP_BUTT, GDK_JOIN_BEVEL); if (fill == false) - gdk_draw_rectangle(m_background, m_gc, FALSE, - vpMath::round(rectangle.getLeft() / m_scale), - vpMath::round(rectangle.getTop() / m_scale), - vpMath::round(rectangle.getWidth() / m_scale), + gdk_draw_rectangle(m_background, m_gc, FALSE, vpMath::round(rectangle.getLeft() / m_scale), + vpMath::round(rectangle.getTop() / m_scale), vpMath::round(rectangle.getWidth() / m_scale), vpMath::round(rectangle.getHeight() / m_scale)); else - gdk_draw_rectangle(m_background, m_gc, TRUE, - vpMath::round(rectangle.getLeft() / m_scale), - vpMath::round(rectangle.getTop() / m_scale), - vpMath::round(rectangle.getWidth() / m_scale), + gdk_draw_rectangle(m_background, m_gc, TRUE, vpMath::round(rectangle.getLeft() / m_scale), + vpMath::round(rectangle.getTop() / m_scale), vpMath::round(rectangle.getWidth() / m_scale), vpMath::round(rectangle.getHeight() / m_scale)); if (thickness > 1) - gdk_gc_set_line_attributes(m_gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT, - GDK_JOIN_BEVEL); + gdk_gc_set_line_attributes(m_gc, 0, GDK_LINE_SOLID, GDK_CAP_BUTT, GDK_JOIN_BEVEL); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -1229,8 +1113,7 @@ bool vpDisplayGTK::getClick(bool blocking) // 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) { + if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS) { ret = true; // printf("Click detection\n"); } @@ -1242,8 +1125,7 @@ bool vpDisplayGTK::getClick(bool blocking) } } while (ret == false && blocking == true); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } return ret; } @@ -1274,8 +1156,7 @@ bool vpDisplayGTK::getClick(vpImagePoint &ip, bool blocking) do { GdkEvent *ev = NULL; while ((ev = gdk_event_get()) != NULL) { - if (ev->any.window == widget->window && - ev->type == GDK_BUTTON_PRESS) { + if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS) { u = ((GdkEventButton *)ev)->x; v = ((GdkEventButton *)ev)->y; ip.set_u(u * m_scale); @@ -1290,8 +1171,7 @@ bool vpDisplayGTK::getClick(vpImagePoint &ip, bool blocking) } } while (ret == false && blocking == true); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } return ret; } @@ -1315,9 +1195,7 @@ bool vpDisplayGTK::getClick(vpImagePoint &ip, bool blocking) 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 vpDisplayGTK::getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { bool ret = false; @@ -1326,8 +1204,7 @@ bool vpDisplayGTK::getClick(vpImagePoint &ip, do { GdkEvent *ev = NULL; while ((ev = gdk_event_get())) { - if (ev->any.window == widget->window && - ev->type == GDK_BUTTON_PRESS) { + if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS) { u = ((GdkEventButton *)ev)->x; v = ((GdkEventButton *)ev)->y; ip.set_u(u * m_scale); @@ -1355,8 +1232,7 @@ bool vpDisplayGTK::getClick(vpImagePoint &ip, } while (ret == false && blocking == true); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } return ret; } @@ -1384,9 +1260,7 @@ bool vpDisplayGTK::getClick(vpImagePoint &ip, \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool) */ -bool vpDisplayGTK::getClickUp(vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, - bool blocking) +bool vpDisplayGTK::getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { bool ret = false; @@ -1397,8 +1271,7 @@ bool vpDisplayGTK::getClickUp(vpImagePoint &ip, do { GdkEvent *ev = NULL; while ((ev = gdk_event_get()) != NULL) { - if (ev->any.window == widget->window && - ev->type == GDK_BUTTON_RELEASE) { + if (ev->any.window == widget->window && ev->type == GDK_BUTTON_RELEASE) { u = ((GdkEventButton *)ev)->x; v = ((GdkEventButton *)ev)->y; ip.set_u(u * m_scale); @@ -1426,8 +1299,7 @@ bool vpDisplayGTK::getClickUp(vpImagePoint &ip, } while (ret == false && blocking == true); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } return ret; } @@ -1445,8 +1317,7 @@ void vpDisplayGTK::getImage(vpImage &I) /* */ - ImageGtk = - gdk_image_get(m_background, 0, 0, (gint)m_width, (gint)m_height); + ImageGtk = gdk_image_get(m_background, 0, 0, (gint)m_width, (gint)m_height); I.resize(m_height, m_width); guint32 pixel; @@ -1467,8 +1338,7 @@ void vpDisplayGTK::getImage(vpImage &I) } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } } @@ -1528,8 +1398,7 @@ bool vpDisplayGTK::getKeyboardEvent(bool blocking) } } while (ret == false && blocking == true); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } return ret; } @@ -1582,8 +1451,7 @@ bool vpDisplayGTK::getKeyboardEvent(std::string &key, bool blocking) } } while (ret == false && blocking == true); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } return ret; } @@ -1618,8 +1486,7 @@ bool vpDisplayGTK::getPointerMotionEvent(vpImagePoint &ip) gdk_event_free(ev); } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } return ret; } @@ -1642,8 +1509,7 @@ bool vpDisplayGTK::getPointerPosition(vpImagePoint &ip) ip.set_u(u * m_scale); ip.set_v(v * m_scale); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "GTK not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized")); } return true; diff --git a/modules/gui/src/display/vpDisplayOpenCV.cpp b/modules/gui/src/display/vpDisplayOpenCV.cpp index 99bb3807e9..a59a4df31c 100644 --- a/modules/gui/src/display/vpDisplayOpenCV.cpp +++ b/modules/gui/src/display/vpDisplayOpenCV.cpp @@ -80,8 +80,7 @@ #include #endif -std::vector vpDisplayOpenCV::m_listTitles = - std::vector(); +std::vector vpDisplayOpenCV::m_listTitles = std::vector(); unsigned int vpDisplayOpenCV::m_nbWindows = 0; /*! @@ -105,20 +104,16 @@ unsigned int vpDisplayOpenCV::m_nbWindows = 0; and the columns. */ -vpDisplayOpenCV::vpDisplayOpenCV(vpImage &I, - vpScaleType scaleType) +vpDisplayOpenCV::vpDisplayOpenCV(vpImage &I, vpScaleType scaleType) : vpDisplay(), #if (VISP_HAVE_OPENCV_VERSION < 0x020408) m_background(NULL), col(NULL), cvcolor(), font(NULL), #else - m_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), + m_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) { setScale(scaleType, I.getWidth(), I.getHeight()); @@ -148,21 +143,17 @@ vpDisplayOpenCV::vpDisplayOpenCV(vpImage &I, and the columns. */ -vpDisplayOpenCV::vpDisplayOpenCV(vpImage &I, int x, int y, - const std::string &title, +vpDisplayOpenCV::vpDisplayOpenCV(vpImage &I, int x, int y, const std::string &title, vpScaleType scaleType) : vpDisplay(), #if (VISP_HAVE_OPENCV_VERSION < 0x020408) m_background(NULL), col(NULL), cvcolor(), font(NULL), #else - m_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), + m_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) { setScale(scaleType, I.getWidth(), I.getHeight()); @@ -192,14 +183,11 @@ vpDisplayOpenCV::vpDisplayOpenCV(vpImage &I, vpScaleType scaleType) #if (VISP_HAVE_OPENCV_VERSION < 0x020408) m_background(NULL), col(NULL), cvcolor(), font(NULL), #else - m_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), + m_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) { setScale(scaleType, I.getWidth(), I.getHeight()); @@ -226,21 +214,16 @@ vpDisplayOpenCV::vpDisplayOpenCV(vpImage &I, vpScaleType scaleType) - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns. */ -vpDisplayOpenCV::vpDisplayOpenCV(vpImage &I, int x, int y, - const std::string &title, - vpScaleType scaleType) +vpDisplayOpenCV::vpDisplayOpenCV(vpImage &I, int x, int y, const std::string &title, vpScaleType scaleType) : #if (VISP_HAVE_OPENCV_VERSION < 0x020408) m_background(NULL), col(NULL), cvcolor(), font(NULL), #else - m_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), + m_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) { setScale(scaleType, I.getWidth(), I.getHeight()); @@ -274,14 +257,11 @@ vpDisplayOpenCV::vpDisplayOpenCV(int x, int y, const std::string &title) #if (VISP_HAVE_OPENCV_VERSION < 0x020408) m_background(NULL), col(NULL), cvcolor(), font(NULL), #else - m_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), + m_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) { m_windowXPosition = x; @@ -336,14 +316,11 @@ vpDisplayOpenCV::vpDisplayOpenCV() #if (VISP_HAVE_OPENCV_VERSION < 0x020408) m_background(NULL), col(NULL), cvcolor(), font(NULL), #else - m_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), + m_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) { } @@ -367,12 +344,10 @@ vpDisplayOpenCV::~vpDisplayOpenCV() \param title : Window title. */ -void vpDisplayOpenCV::init(vpImage &I, int x, int y, - const std::string &title) +void vpDisplayOpenCV::init(vpImage &I, int x, int y, const std::string &title) { if ((I.getHeight() == 0) || (I.getWidth() == 0)) { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "Image not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized")); } setScale(m_scaleType, I.getWidth(), I.getHeight()); init(I.getWidth(), I.getHeight(), x, y, title); @@ -389,12 +364,10 @@ void vpDisplayOpenCV::init(vpImage &I, int x, int y, \param title : Window title. */ -void vpDisplayOpenCV::init(vpImage &I, int x, int y, - const std::string &title) +void vpDisplayOpenCV::init(vpImage &I, int x, int y, const std::string &title) { if ((I.getHeight() == 0) || (I.getWidth() == 0)) { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "Image not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized")); } setScale(m_scaleType, I.getWidth(), I.getHeight()); @@ -413,8 +386,7 @@ void vpDisplayOpenCV::init(vpImage &I, int x, int y, \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 std::string &title) +void vpDisplayOpenCV::init(unsigned int w, unsigned int h, int x, int y, const std::string &title) { setScale(m_scaleType, w, h); @@ -461,18 +433,15 @@ void vpDisplayOpenCV::init(unsigned int w, unsigned int h, int x, int y, /* Create the window*/ #if (VISP_HAVE_OPENCV_VERSION < 0x020408) if (cvNamedWindow(this->m_title.c_str(), flags) < 0) { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV was not built with a display device")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV was not built with a display device")); } #else cv::namedWindow(this->m_title, flags); #endif #if (VISP_HAVE_OPENCV_VERSION < 0x020408) - cvMoveWindow(this->m_title.c_str(), this->m_windowXPosition, - this->m_windowYPosition); + cvMoveWindow(this->m_title.c_str(), this->m_windowXPosition, this->m_windowYPosition); #else - cv::moveWindow(this->m_title.c_str(), this->m_windowXPosition, - this->m_windowYPosition); + cv::moveWindow(this->m_title.c_str(), this->m_windowXPosition, this->m_windowYPosition); #endif move = false; lbuttondown = false; @@ -560,10 +529,7 @@ void vpDisplayOpenCV::init(unsigned int w, unsigned int h, int x, int y, \sa displayCharString() */ -void vpDisplayOpenCV::setFont(const std::string & /* font */) -{ - vpERROR_TRACE("Not yet implemented"); -} +void vpDisplayOpenCV::setFont(const std::string & /* font */) { vpERROR_TRACE("Not yet implemented"); } /*! Set the window title. @@ -601,8 +567,7 @@ void vpDisplayOpenCV::setWindowPosition(int winx, int winy) cv::moveWindow(this->m_title.c_str(), winx, winy); #endif } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } /*! @@ -624,10 +589,8 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) int channels = 3; CvSize size = cvSize((int)this->m_width, (int)this->m_height); if (m_background != NULL) { - if (m_background->nChannels != channels || - m_background->depth != depth || - m_background->height != (int)m_height || - m_background->width != (int)m_width) { + if (m_background->nChannels != channels || m_background->depth != depth || + m_background->height != (int)m_height || m_background->width != (int)m_width) { if (m_background->nChannels != 0) cvReleaseImage(&m_background); m_background = cvCreateImage(size, depth, channels); @@ -638,8 +601,7 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) if (m_scale == 1) { for (unsigned int i = 0; i < m_height; i++) { - unsigned char *dst_24 = (unsigned char *)m_background->imageData + - (int)(i * m_background->widthStep); + unsigned char *dst_24 = (unsigned char *)m_background->imageData + (int)(i * m_background->widthStep); for (unsigned int j = 0; j < m_width; j++) { unsigned char val = I[i][j]; *(dst_24++) = val; @@ -649,8 +611,7 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) } } else { for (unsigned int i = 0; i < m_height; i++) { - unsigned char *dst_24 = (unsigned char *)m_background->imageData + - (int)(i * m_background->widthStep); + unsigned char *dst_24 = (unsigned char *)m_background->imageData + (int)(i * m_background->widthStep); for (unsigned int j = 0; j < m_width; j++) { unsigned char val = I[i * m_scale][j * m_scale]; *(dst_24++) = val; @@ -664,16 +625,14 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) int depth = CV_8U; int channels = 3; cv::Size size((int)m_width, (int)m_height); - if (m_background.channels() != channels || - m_background.depth() != depth || m_background.rows != (int)m_height || + if (m_background.channels() != channels || m_background.depth() != depth || m_background.rows != (int)m_height || m_background.cols != (int)m_width) { m_background = cv::Mat(size, CV_MAKETYPE(depth, channels)); } if (m_scale == 1) { for (unsigned int i = 0; i < m_height; i++) { - unsigned char *dst_24 = - (unsigned char *)m_background.data + (int)(i * 3 * m_width); + unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width); for (unsigned int j = 0; j < m_width; j++) { unsigned char val = I[i][j]; *(dst_24++) = val; @@ -683,8 +642,7 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) } } else { for (unsigned int i = 0; i < m_height; i++) { - unsigned char *dst_24 = - (unsigned char *)m_background.data + (int)(i * 3 * m_width); + unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width); for (unsigned int j = 0; j < m_width; j++) { unsigned char val = I[i * m_scale][j * m_scale]; *(dst_24++) = val; @@ -696,8 +654,7 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) #endif } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -716,9 +673,7 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) \sa init(), closeDisplay() */ -void vpDisplayOpenCV::displayImageROI(const vpImage &I, - const vpImagePoint &iP, - const unsigned int w, +void vpDisplayOpenCV::displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int w, const unsigned int h) { if (m_displayHasBeenInitialized) { @@ -727,10 +682,8 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, int channels = 3; CvSize size = cvSize((int)this->m_width, (int)this->m_height); if (m_background != NULL) { - if (m_background->nChannels != channels || - m_background->depth != depth || - m_background->height != (int)m_height || - m_background->width != (int)m_width) { + if (m_background->nChannels != channels || m_background->depth != depth || + m_background->height != (int)m_height || m_background->width != (int)m_width) { if (m_background->nChannels != 0) cvReleaseImage(&m_background); m_background = cvCreateImage(size, depth, channels); @@ -746,8 +699,7 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, unsigned int j_max = (std::min)(j_min + w, m_width); for (unsigned int i = i_min; i < i_max; i++) { unsigned char *dst_24 = - (unsigned char *)m_background->imageData + - (int)(i * m_background->widthStep + j_min * 3); + (unsigned char *)m_background->imageData + (int)(i * m_background->widthStep + j_min * 3); for (unsigned int j = j_min; j < j_max; j++) { unsigned char val = I[i][j]; *(dst_24++) = val; @@ -758,14 +710,11 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, } else { int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = - (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = - (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); for (int i = i_min; i < i_max; i++) { unsigned char *dst_24 = - (unsigned char *)m_background->imageData + - (int)(i * m_background->widthStep + j_min * 3); + (unsigned char *)m_background->imageData + (int)(i * m_background->widthStep + j_min * 3); for (int j = j_min; j < j_max; j++) { unsigned char val = I[i * m_scale][j * m_scale]; *(dst_24++) = val; @@ -779,8 +728,7 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, int depth = CV_8U; int channels = 3; cv::Size size((int)m_width, (int)m_height); - if (m_background.channels() != channels || - m_background.depth() != depth || m_background.rows != (int)m_height || + if (m_background.channels() != channels || m_background.depth() != depth || m_background.rows != (int)m_height || m_background.cols != (int)m_width) { m_background = cv::Mat(size, CV_MAKETYPE(depth, channels)); } @@ -791,8 +739,7 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, unsigned int i_max = (std::min)(i_min + h, m_height); unsigned int j_max = (std::min)(j_min + w, m_width); for (unsigned int i = i_min; i < i_max; i++) { - unsigned char *dst_24 = (unsigned char *)m_background.data + - (int)(i * 3 * m_width + j_min * 3); + unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width + j_min * 3); for (unsigned int j = j_min; j < j_max; j++) { unsigned char val = I[i][j]; *(dst_24++) = val; @@ -803,13 +750,10 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, } else { int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = - (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = - (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); for (int i = i_min; i < i_max; i++) { - unsigned char *dst_24 = (unsigned char *)m_background.data + - (int)(i * 3 * m_width + j_min * 3); + unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width + j_min * 3); for (int j = j_min; j < j_max; j++) { unsigned char val = I[i * m_scale][j * m_scale]; *(dst_24++) = val; @@ -820,8 +764,7 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, } #endif } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -845,10 +788,8 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) int channels = 3; CvSize size = cvSize((int)this->m_width, (int)this->m_height); if (m_background != NULL) { - if (m_background->nChannels != channels || - m_background->depth != depth || - m_background->height != (int)m_height || - m_background->width != (int)m_width) { + if (m_background->nChannels != channels || m_background->depth != depth || + m_background->height != (int)m_height || m_background->width != (int)m_width) { if (m_background->nChannels != 0) cvReleaseImage(&m_background); m_background = cvCreateImage(size, depth, channels); @@ -859,8 +800,7 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) if (m_scale == 1) { for (unsigned int i = 0; i < m_height; i++) { - unsigned char *dst_24 = (unsigned char *)m_background->imageData + - (int)(i * m_background->widthStep); + unsigned char *dst_24 = (unsigned char *)m_background->imageData + (int)(i * m_background->widthStep); for (unsigned int j = 0; j < m_width; j++) { vpRGBa val = I[i][j]; *(dst_24++) = val.B; @@ -870,8 +810,7 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) } } else { for (unsigned int i = 0; i < m_height; i++) { - unsigned char *dst_24 = (unsigned char *)m_background->imageData + - (int)(i * m_background->widthStep); + unsigned char *dst_24 = (unsigned char *)m_background->imageData + (int)(i * m_background->widthStep); for (unsigned int j = 0; j < m_width; j++) { vpRGBa val = I[i * m_scale][j * m_scale]; *(dst_24++) = val.B; @@ -884,16 +823,14 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) int depth = CV_8U; int channels = 3; cv::Size size((int)this->m_width, (int)this->m_height); - if (m_background.channels() != channels || - m_background.depth() != depth || m_background.rows != (int)m_height || + if (m_background.channels() != channels || m_background.depth() != depth || m_background.rows != (int)m_height || m_background.cols != (int)m_width) { m_background = cv::Mat(size, CV_MAKETYPE(depth, channels)); } if (m_scale == 1) { for (unsigned int i = 0; i < m_height; i++) { - unsigned char *dst_24 = - (unsigned char *)m_background.data + (int)(i * 3 * m_width); + unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width); for (unsigned int j = 0; j < m_width; j++) { vpRGBa val = I[i][j]; *(dst_24++) = val.B; @@ -903,8 +840,7 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) } } else { for (unsigned int i = 0; i < m_height; i++) { - unsigned char *dst_24 = - (unsigned char *)m_background.data + (int)(i * 3 * m_width); + unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width); for (unsigned int j = 0; j < m_width; j++) { vpRGBa val = I[i * m_scale][j * m_scale]; *(dst_24++) = val.B; @@ -915,8 +851,7 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) } #endif } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -935,9 +870,7 @@ void vpDisplayOpenCV::displayImage(const vpImage &I) \sa init(), closeDisplay() */ -void vpDisplayOpenCV::displayImageROI(const vpImage &I, - const vpImagePoint &iP, - const unsigned int w, +void vpDisplayOpenCV::displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int w, const unsigned int h) { if (m_displayHasBeenInitialized) { @@ -946,10 +879,8 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, int channels = 3; CvSize size = cvSize((int)this->m_width, (int)this->m_height); if (m_background != NULL) { - if (m_background->nChannels != channels || - m_background->depth != depth || - m_background->height != (int)m_height || - m_background->width != (int)m_width) { + if (m_background->nChannels != channels || m_background->depth != depth || + m_background->height != (int)m_height || m_background->width != (int)m_width) { if (m_background->nChannels != 0) cvReleaseImage(&m_background); m_background = cvCreateImage(size, depth, channels); @@ -965,8 +896,7 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, unsigned int j_max = (std::min)(j_min + w, m_width); for (unsigned int i = i_min; i < i_max; i++) { unsigned char *dst_24 = - (unsigned char *)m_background->imageData + - (int)(i * m_background->widthStep + j_min * 3); + (unsigned char *)m_background->imageData + (int)(i * m_background->widthStep + j_min * 3); for (unsigned int j = j_min; j < j_max; j++) { vpRGBa val = I[i][j]; *(dst_24++) = val.B; @@ -977,14 +907,11 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, } else { int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = - (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = - (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); for (int i = i_min; i < i_max; i++) { unsigned char *dst_24 = - (unsigned char *)m_background->imageData + - (int)(i * m_background->widthStep + j_min * 3); + (unsigned char *)m_background->imageData + (int)(i * m_background->widthStep + j_min * 3); for (int j = j_min; j < j_max; j++) { vpRGBa val = I[i * m_scale][j * m_scale]; *(dst_24++) = val.B; @@ -997,8 +924,7 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, int depth = CV_8U; int channels = 3; cv::Size size((int)this->m_width, (int)this->m_height); - if (m_background.channels() != channels || - m_background.depth() != depth || m_background.rows != (int)m_height || + if (m_background.channels() != channels || m_background.depth() != depth || m_background.rows != (int)m_height || m_background.cols != (int)m_width) { m_background = cv::Mat(size, CV_MAKETYPE(depth, channels)); } @@ -1009,8 +935,7 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, unsigned int i_max = (std::min)(i_min + h, m_height); unsigned int j_max = (std::min)(j_min + w, m_width); for (unsigned int i = i_min; i < i_max; i++) { - unsigned char *dst_24 = (unsigned char *)m_background.data + - (int)(i * 3 * m_width + j_min * 3); + unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width + j_min * 3); for (unsigned int j = j_min; j < j_max; j++) { vpRGBa val = I[i][j]; *(dst_24++) = val.B; @@ -1021,13 +946,10 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, } else { int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = - (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = - (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); for (int i = i_min; i < i_max; i++) { - unsigned char *dst_24 = (unsigned char *)m_background.data + - (int)(i * 3 * m_width + j_min * 3); + unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width + j_min * 3); for (int j = j_min; j < j_max; j++) { vpRGBa val = I[i * m_scale][j * m_scale]; *(dst_24++) = val.B; @@ -1038,8 +960,7 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, } #endif } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -1048,10 +969,7 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, \sa init(), closeDisplay() */ -void vpDisplayOpenCV::displayImage(const unsigned char * /* I */) -{ - vpTRACE(" not implemented "); -} +void vpDisplayOpenCV::displayImage(const unsigned char * /* I */) { vpTRACE(" not implemented "); } /*! @@ -1108,8 +1026,7 @@ void vpDisplayOpenCV::flushDisplay() cv::waitKey(5); #endif } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -1118,8 +1035,7 @@ void vpDisplayOpenCV::flushDisplay() It's necessary to use this function to see the results of any drawing. */ -void vpDisplayOpenCV::flushDisplayROI(const vpImagePoint & /*iP*/, - const unsigned int /*width*/, +void vpDisplayOpenCV::flushDisplayROI(const vpImagePoint & /*iP*/, const unsigned int /*width*/, const unsigned int /*height*/) { if (m_displayHasBeenInitialized) { @@ -1131,8 +1047,7 @@ void vpDisplayOpenCV::flushDisplayROI(const vpImagePoint & /*iP*/, cv::waitKey(5); #endif } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -1155,10 +1070,8 @@ void vpDisplayOpenCV::clearDisplay(const vpColor & /* 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) +void vpDisplayOpenCV::displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, + unsigned int w, unsigned int h, unsigned int thickness) { if (m_displayHasBeenInitialized) { double a = ip2.get_i() - ip1.get_i(); @@ -1193,8 +1106,7 @@ void vpDisplayOpenCV::displayArrow(const vpImagePoint &ip1, displayLine(ip1, ip2, color, thickness); } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -1209,40 +1121,33 @@ void vpDisplayOpenCV::displayArrow(const vpImagePoint &ip1, \sa setFont() */ -void vpDisplayOpenCV::displayCharString(const vpImagePoint &ip, - const char *text, - const vpColor &color) +void vpDisplayOpenCV::displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color) { if (m_displayHasBeenInitialized) { if (color.id < vpColor::id_unknown) { #if VISP_HAVE_OPENCV_VERSION < 0x020408 cvPutText(m_background, text, - cvPoint(vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale + fontHeight)), - font, col[color.id]); + cvPoint(vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale + fontHeight)), font, + col[color.id]); #else cv::putText(m_background, text, - cv::Point(vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale + fontHeight)), + cv::Point(vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale + fontHeight)), font, fontScale, col[color.id]); #endif } else { cvcolor = CV_RGB(color.R, color.G, color.B); #if VISP_HAVE_OPENCV_VERSION < 0x020408 cvPutText(m_background, text, - cvPoint(vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale + fontHeight)), - font, cvcolor); + cvPoint(vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale + fontHeight)), font, + cvcolor); #else cv::putText(m_background, text, - cv::Point(vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale + fontHeight)), + cv::Point(vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale + fontHeight)), font, fontScale, cvcolor); #endif } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } /*! @@ -1254,35 +1159,30 @@ void vpDisplayOpenCV::displayCharString(const vpImagePoint &ip, \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) +void vpDisplayOpenCV::displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill, + unsigned int thickness) { if (m_displayHasBeenInitialized) { if (fill == false) { if (color.id < vpColor::id_unknown) { #if VISP_HAVE_OPENCV_VERSION < 0x020408 cvCircle(m_background, - cvPoint(vpMath::round(center.get_u() / m_scale), - vpMath::round(center.get_v() / m_scale)), + cvPoint(vpMath::round(center.get_u() / m_scale), vpMath::round(center.get_v() / m_scale)), (int)radius / m_scale, col[color.id], (int)thickness); #else cv::circle(m_background, - cv::Point(vpMath::round(center.get_u() / m_scale), - vpMath::round(center.get_v() / m_scale)), + cv::Point(vpMath::round(center.get_u() / m_scale), vpMath::round(center.get_v() / m_scale)), (int)radius / m_scale, col[color.id], (int)thickness); #endif } else { cvcolor = CV_RGB(color.R, color.G, color.B); #if VISP_HAVE_OPENCV_VERSION < 0x020408 cvCircle(m_background, - cvPoint(vpMath::round(center.get_u() / m_scale), - vpMath::round(center.get_v() / m_scale)), + cvPoint(vpMath::round(center.get_u() / m_scale), vpMath::round(center.get_v() / m_scale)), (int)radius / m_scale, cvcolor, (int)thickness); #else cv::circle(m_background, - cv::Point(vpMath::round(center.get_u() / m_scale), - vpMath::round(center.get_v() / m_scale)), + cv::Point(vpMath::round(center.get_u() / m_scale), vpMath::round(center.get_v() / m_scale)), (int)radius / m_scale, cvcolor, (int)thickness); #endif } @@ -1295,33 +1195,28 @@ void vpDisplayOpenCV::displayCircle(const vpImagePoint ¢er, if (color.id < vpColor::id_unknown) { #if VISP_HAVE_OPENCV_VERSION < 0x020408 cvCircle(m_background, - cvPoint(vpMath::round(center.get_u() / m_scale), - vpMath::round(center.get_v() / m_scale)), + cvPoint(vpMath::round(center.get_u() / m_scale), vpMath::round(center.get_v() / m_scale)), (int)radius / m_scale, col[color.id], filled); #else cv::circle(m_background, - cv::Point(vpMath::round(center.get_u() / m_scale), - vpMath::round(center.get_v() / m_scale)), + cv::Point(vpMath::round(center.get_u() / m_scale), vpMath::round(center.get_v() / m_scale)), (int)radius / m_scale, col[color.id], filled); #endif } else { cvcolor = CV_RGB(color.R, color.G, color.B); #if VISP_HAVE_OPENCV_VERSION < 0x020408 cvCircle(m_background, - cvPoint(vpMath::round(center.get_u() / m_scale), - vpMath::round(center.get_v() / m_scale)), + cvPoint(vpMath::round(center.get_u() / m_scale), vpMath::round(center.get_v() / m_scale)), (int)radius / m_scale, cvcolor, filled); #else cv::circle(m_background, - cv::Point(vpMath::round(center.get_u() / m_scale), - vpMath::round(center.get_v() / m_scale)), + cv::Point(vpMath::round(center.get_u() / m_scale), vpMath::round(center.get_v() / m_scale)), (int)radius / m_scale, cvcolor, filled); #endif } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -1332,8 +1227,7 @@ void vpDisplayOpenCV::displayCircle(const vpImagePoint ¢er, \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, +void vpDisplayOpenCV::displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness) { if (m_displayHasBeenInitialized) { @@ -1349,8 +1243,7 @@ void vpDisplayOpenCV::displayCross(const vpImagePoint &ip, unsigned int size, displayLine(top, bottom, color, thickness); displayLine(left, right, color, thickness); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -1361,28 +1254,22 @@ void vpDisplayOpenCV::displayCross(const vpImagePoint &ip, unsigned int size, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplayOpenCV::displayDotLine(const vpImagePoint &ip1, - const vpImagePoint &ip2, - const vpColor &color, +void vpDisplayOpenCV::displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness) { if (m_displayHasBeenInitialized) { double size = 10. * m_scale; - double length = sqrt(vpMath::sqr(ip2.get_i() - ip1.get_i()) + - vpMath::sqr(ip2.get_j() - ip1.get_j())); + double length = sqrt(vpMath::sqr(ip2.get_i() - ip1.get_i()) + vpMath::sqr(ip2.get_j() - ip1.get_j())); double deltaj = size / length * (ip2.get_j() - ip1.get_j()); double deltai = size / length * (ip2.get_i() - ip1.get_i()); double slope = (ip2.get_i() - ip1.get_i()) / (ip2.get_j() - ip1.get_j()); double orig = ip1.get_i() - slope * ip1.get_j(); - for (unsigned int j = (unsigned int)ip1.get_j(); j < ip2.get_j(); - j += (unsigned int)(2 * deltaj)) { + for (unsigned int j = (unsigned int)ip1.get_j(); j < ip2.get_j(); j += (unsigned int)(2 * deltaj)) { double i = slope * j + orig; - displayLine(vpImagePoint(i, j), vpImagePoint(i + deltai, j + deltaj), - color, thickness); + displayLine(vpImagePoint(i, j), vpImagePoint(i + deltai, j + deltaj), color, thickness); } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -1392,49 +1279,34 @@ void vpDisplayOpenCV::displayDotLine(const vpImagePoint &ip1, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplayOpenCV::displayLine(const vpImagePoint &ip1, - const vpImagePoint &ip2, - const vpColor &color, +void vpDisplayOpenCV::displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness) { if (m_displayHasBeenInitialized) { if (color.id < vpColor::id_unknown) { #if VISP_HAVE_OPENCV_VERSION < 0x020408 - cvLine(m_background, - cvPoint(vpMath::round(ip1.get_u() / m_scale), - vpMath::round(ip1.get_v() / m_scale)), - cvPoint(vpMath::round(ip2.get_u() / m_scale), - vpMath::round(ip2.get_v() / m_scale)), - col[color.id], (int)thickness); + cvLine(m_background, cvPoint(vpMath::round(ip1.get_u() / m_scale), vpMath::round(ip1.get_v() / m_scale)), + cvPoint(vpMath::round(ip2.get_u() / m_scale), vpMath::round(ip2.get_v() / m_scale)), col[color.id], + (int)thickness); #else - cv::line(m_background, - cv::Point(vpMath::round(ip1.get_u() / m_scale), - vpMath::round(ip1.get_v() / m_scale)), - cv::Point(vpMath::round(ip2.get_u() / m_scale), - vpMath::round(ip2.get_v() / m_scale)), - col[color.id], (int)thickness); + cv::line(m_background, cv::Point(vpMath::round(ip1.get_u() / m_scale), vpMath::round(ip1.get_v() / m_scale)), + cv::Point(vpMath::round(ip2.get_u() / m_scale), vpMath::round(ip2.get_v() / m_scale)), col[color.id], + (int)thickness); #endif } else { cvcolor = CV_RGB(color.R, color.G, color.B); #if VISP_HAVE_OPENCV_VERSION < 0x020408 - cvLine(m_background, - cvPoint(vpMath::round(ip1.get_u() / m_scale), - vpMath::round(ip1.get_v() / m_scale)), - cvPoint(vpMath::round(ip2.get_u() / m_scale), - vpMath::round(ip2.get_v() / m_scale)), - cvcolor, (int)thickness); + cvLine(m_background, cvPoint(vpMath::round(ip1.get_u() / m_scale), vpMath::round(ip1.get_v() / m_scale)), + cvPoint(vpMath::round(ip2.get_u() / m_scale), vpMath::round(ip2.get_v() / m_scale)), cvcolor, + (int)thickness); #else - cv::line(m_background, - cv::Point(vpMath::round(ip1.get_u() / m_scale), - vpMath::round(ip1.get_v() / m_scale)), - cv::Point(vpMath::round(ip2.get_u() / m_scale), - vpMath::round(ip2.get_v() / m_scale)), - cvcolor, (int)thickness); + cv::line(m_background, cv::Point(vpMath::round(ip1.get_u() / m_scale), vpMath::round(ip1.get_v() / m_scale)), + cv::Point(vpMath::round(ip2.get_u() / m_scale), vpMath::round(ip2.get_v() / m_scale)), cvcolor, + (int)thickness); #endif } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -1444,52 +1316,35 @@ void vpDisplayOpenCV::displayLine(const vpImagePoint &ip1, \param color : Point color. \param thickness : point thickness. */ -void vpDisplayOpenCV::displayPoint(const vpImagePoint &ip, - const vpColor &color, - unsigned int thickness) +void vpDisplayOpenCV::displayPoint(const vpImagePoint &ip, const vpColor &color, unsigned int thickness) { if (m_displayHasBeenInitialized) { for (unsigned int i = 0; i < thickness; i++) { if (color.id < vpColor::id_unknown) { #if VISP_HAVE_OPENCV_VERSION < 0x020408 - cvLine(m_background, - cvPoint(vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale)), - cvPoint(vpMath::round(ip.get_u() / m_scale + thickness - 1), - vpMath::round(ip.get_v() / m_scale)), + cvLine(m_background, cvPoint(vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale)), + cvPoint(vpMath::round(ip.get_u() / m_scale + thickness - 1), vpMath::round(ip.get_v() / m_scale)), col[color.id], (int)thickness); #else - cv::line( - m_background, - cv::Point(vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale)), - cv::Point(vpMath::round(ip.get_u() / m_scale + thickness - 1), - vpMath::round(ip.get_v() / m_scale)), - col[color.id], (int)thickness); + cv::line(m_background, cv::Point(vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale)), + cv::Point(vpMath::round(ip.get_u() / m_scale + thickness - 1), vpMath::round(ip.get_v() / m_scale)), + col[color.id], (int)thickness); #endif } else { cvcolor = CV_RGB(color.R, color.G, color.B); #if VISP_HAVE_OPENCV_VERSION < 0x020408 - cvLine(m_background, - cvPoint(vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale)), - cvPoint(vpMath::round(ip.get_u() / m_scale + thickness - 1), - vpMath::round(ip.get_v() / m_scale)), + cvLine(m_background, cvPoint(vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale)), + cvPoint(vpMath::round(ip.get_u() / m_scale + thickness - 1), vpMath::round(ip.get_v() / m_scale)), cvcolor, (int)thickness); #else - cv::line( - m_background, - cv::Point(vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale)), - cv::Point(vpMath::round(ip.get_u() / m_scale + thickness - 1), - vpMath::round(ip.get_v() / m_scale)), - cvcolor, (int)thickness); + cv::line(m_background, cv::Point(vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale)), + cv::Point(vpMath::round(ip.get_u() / m_scale + thickness - 1), vpMath::round(ip.get_v() / m_scale)), + cvcolor, (int)thickness); #endif } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -1506,46 +1361,34 @@ void vpDisplayOpenCV::displayPoint(const vpImagePoint &ip, 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) +void vpDisplayOpenCV::displayRectangle(const vpImagePoint &topLeft, unsigned int w, unsigned int h, + const vpColor &color, bool fill, unsigned int thickness) { if (m_displayHasBeenInitialized) { if (fill == false) { if (color.id < vpColor::id_unknown) { #if VISP_HAVE_OPENCV_VERSION < 0x020408 - cvRectangle(m_background, - cvPoint(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cvPoint(vpMath::round((topLeft.get_u() + w) / m_scale), - vpMath::round((topLeft.get_v() + h) / m_scale)), - col[color.id], (int)thickness); + cvRectangle( + m_background, cvPoint(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cvPoint(vpMath::round((topLeft.get_u() + w) / m_scale), vpMath::round((topLeft.get_v() + h) / m_scale)), + col[color.id], (int)thickness); #else cv::rectangle( - m_background, - cv::Point(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cv::Point(vpMath::round((topLeft.get_u() + w) / m_scale), - vpMath::round((topLeft.get_v() + h) / m_scale)), + m_background, cv::Point(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cv::Point(vpMath::round((topLeft.get_u() + w) / m_scale), vpMath::round((topLeft.get_v() + h) / m_scale)), col[color.id], (int)thickness); #endif } else { cvcolor = CV_RGB(color.R, color.G, color.B); #if VISP_HAVE_OPENCV_VERSION < 0x020408 - cvRectangle(m_background, - cvPoint(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cvPoint(vpMath::round((topLeft.get_u() + w) / m_scale), - vpMath::round((topLeft.get_v() + h) / m_scale)), - cvcolor, (int)thickness); + cvRectangle( + m_background, cvPoint(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cvPoint(vpMath::round((topLeft.get_u() + w) / m_scale), vpMath::round((topLeft.get_v() + h) / m_scale)), + cvcolor, (int)thickness); #else cv::rectangle( - m_background, - cv::Point(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cv::Point(vpMath::round((topLeft.get_u() + w) / m_scale), - vpMath::round((topLeft.get_v() + h) / m_scale)), + m_background, cv::Point(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cv::Point(vpMath::round((topLeft.get_u() + w) / m_scale), vpMath::round((topLeft.get_v() + h) / m_scale)), cvcolor, (int)thickness); #endif } @@ -1557,44 +1400,33 @@ void vpDisplayOpenCV::displayRectangle(const vpImagePoint &topLeft, #endif if (color.id < vpColor::id_unknown) { #if VISP_HAVE_OPENCV_VERSION < 0x020408 - cvRectangle(m_background, - cvPoint(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cvPoint(vpMath::round((topLeft.get_u() + w) / m_scale), - vpMath::round((topLeft.get_v() + h) / m_scale)), - col[color.id], filled); + cvRectangle( + m_background, cvPoint(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cvPoint(vpMath::round((topLeft.get_u() + w) / m_scale), vpMath::round((topLeft.get_v() + h) / m_scale)), + col[color.id], filled); #else cv::rectangle( - m_background, - cv::Point(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cv::Point(vpMath::round((topLeft.get_u() + w) / m_scale), - vpMath::round((topLeft.get_v() + h) / m_scale)), + m_background, cv::Point(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cv::Point(vpMath::round((topLeft.get_u() + w) / m_scale), vpMath::round((topLeft.get_v() + h) / m_scale)), col[color.id], filled); #endif } else { cvcolor = CV_RGB(color.R, color.G, color.B); #if VISP_HAVE_OPENCV_VERSION < 0x020408 - cvRectangle(m_background, - cvPoint(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cvPoint(vpMath::round((topLeft.get_u() + w) / m_scale), - vpMath::round((topLeft.get_v() + h) / m_scale)), - cvcolor, filled); + cvRectangle( + m_background, cvPoint(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cvPoint(vpMath::round((topLeft.get_u() + w) / m_scale), vpMath::round((topLeft.get_v() + h) / m_scale)), + cvcolor, filled); #else cv::rectangle( - m_background, - cv::Point(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cv::Point(vpMath::round((topLeft.get_u() + w) / m_scale), - vpMath::round((topLeft.get_v() + h) / m_scale)), + m_background, cv::Point(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cv::Point(vpMath::round((topLeft.get_u() + w) / m_scale), vpMath::round((topLeft.get_v() + h) / m_scale)), cvcolor, filled); #endif } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } /*! @@ -1609,45 +1441,35 @@ void vpDisplayOpenCV::displayRectangle(const vpImagePoint &topLeft, 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) +void vpDisplayOpenCV::displayRectangle(const vpImagePoint &topLeft, const vpImagePoint &bottomRight, + const vpColor &color, bool fill, unsigned int thickness) { if (m_displayHasBeenInitialized) { if (fill == false) { if (color.id < vpColor::id_unknown) { #if VISP_HAVE_OPENCV_VERSION < 0x020408 cvRectangle(m_background, - cvPoint(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cvPoint(vpMath::round(bottomRight.get_u() / m_scale), - vpMath::round(bottomRight.get_v() / m_scale)), + cvPoint(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cvPoint(vpMath::round(bottomRight.get_u() / m_scale), vpMath::round(bottomRight.get_v() / m_scale)), col[color.id], (int)thickness); #else - cv::rectangle(m_background, - cv::Point(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cv::Point(vpMath::round(bottomRight.get_u() / m_scale), - vpMath::round(bottomRight.get_v() / m_scale)), - col[color.id], (int)thickness); + cv::rectangle( + m_background, cv::Point(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cv::Point(vpMath::round(bottomRight.get_u() / m_scale), vpMath::round(bottomRight.get_v() / m_scale)), + col[color.id], (int)thickness); #endif } else { cvcolor = CV_RGB(color.R, color.G, color.B); #if VISP_HAVE_OPENCV_VERSION < 0x020408 cvRectangle(m_background, - cvPoint(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cvPoint(vpMath::round(bottomRight.get_u() / m_scale), - vpMath::round(bottomRight.get_v() / m_scale)), + cvPoint(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cvPoint(vpMath::round(bottomRight.get_u() / m_scale), vpMath::round(bottomRight.get_v() / m_scale)), cvcolor, (int)thickness); #else - cv::rectangle(m_background, - cv::Point(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cv::Point(vpMath::round(bottomRight.get_u() / m_scale), - vpMath::round(bottomRight.get_v() / m_scale)), - cvcolor, (int)thickness); + cv::rectangle( + m_background, cv::Point(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cv::Point(vpMath::round(bottomRight.get_u() / m_scale), vpMath::round(bottomRight.get_v() / m_scale)), + cvcolor, (int)thickness); #endif } } else { @@ -1659,41 +1481,32 @@ void vpDisplayOpenCV::displayRectangle(const vpImagePoint &topLeft, if (color.id < vpColor::id_unknown) { #if VISP_HAVE_OPENCV_VERSION < 0x020408 cvRectangle(m_background, - cvPoint(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cvPoint(vpMath::round(bottomRight.get_u() / m_scale), - vpMath::round(bottomRight.get_v() / m_scale)), + cvPoint(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cvPoint(vpMath::round(bottomRight.get_u() / m_scale), vpMath::round(bottomRight.get_v() / m_scale)), col[color.id], filled); #else - cv::rectangle(m_background, - cv::Point(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cv::Point(vpMath::round(bottomRight.get_u() / m_scale), - vpMath::round(bottomRight.get_v() / m_scale)), - col[color.id], filled); + cv::rectangle( + m_background, cv::Point(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cv::Point(vpMath::round(bottomRight.get_u() / m_scale), vpMath::round(bottomRight.get_v() / m_scale)), + col[color.id], filled); #endif } else { cvcolor = CV_RGB(color.R, color.G, color.B); #if VISP_HAVE_OPENCV_VERSION < 0x020408 cvRectangle(m_background, - cvPoint(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cvPoint(vpMath::round(bottomRight.get_u() / m_scale), - vpMath::round(bottomRight.get_v() / m_scale)), + cvPoint(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cvPoint(vpMath::round(bottomRight.get_u() / m_scale), vpMath::round(bottomRight.get_v() / m_scale)), cvcolor, filled); #else - cv::rectangle(m_background, - cv::Point(vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale)), - cv::Point(vpMath::round(bottomRight.get_u() / m_scale), - vpMath::round(bottomRight.get_v() / m_scale)), - cvcolor, filled); + cv::rectangle( + m_background, cv::Point(vpMath::round(topLeft.get_u() / m_scale), vpMath::round(topLeft.get_v() / m_scale)), + cv::Point(vpMath::round(bottomRight.get_u() / m_scale), vpMath::round(bottomRight.get_v() / m_scale)), + cvcolor, filled); #endif } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -1709,46 +1522,38 @@ void vpDisplayOpenCV::displayRectangle(const vpImagePoint &topLeft, false. */ -void vpDisplayOpenCV::displayRectangle(const vpRect &rectangle, - const vpColor &color, bool fill, - unsigned int thickness) +void vpDisplayOpenCV::displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill, unsigned int thickness) { if (m_displayHasBeenInitialized) { if (fill == false) { if (color.id < vpColor::id_unknown) { #if VISP_HAVE_OPENCV_VERSION < 0x020408 - cvRectangle(m_background, - cvPoint(vpMath::round(rectangle.getLeft() / m_scale), - vpMath::round(rectangle.getBottom() / m_scale)), - cvPoint(vpMath::round(rectangle.getRight() / m_scale), - vpMath::round(rectangle.getTop() / m_scale)), - col[color.id], (int)thickness); + cvRectangle( + m_background, + cvPoint(vpMath::round(rectangle.getLeft() / m_scale), vpMath::round(rectangle.getBottom() / m_scale)), + cvPoint(vpMath::round(rectangle.getRight() / m_scale), vpMath::round(rectangle.getTop() / m_scale)), + col[color.id], (int)thickness); #else cv::rectangle( m_background, - cv::Point(vpMath::round(rectangle.getLeft() / m_scale), - vpMath::round(rectangle.getBottom() / m_scale)), - cv::Point(vpMath::round(rectangle.getRight() / m_scale), - vpMath::round(rectangle.getTop() / m_scale)), + cv::Point(vpMath::round(rectangle.getLeft() / m_scale), vpMath::round(rectangle.getBottom() / m_scale)), + cv::Point(vpMath::round(rectangle.getRight() / m_scale), vpMath::round(rectangle.getTop() / m_scale)), col[color.id], (int)thickness); #endif } else { cvcolor = CV_RGB(color.R, color.G, color.B); #if VISP_HAVE_OPENCV_VERSION < 0x020408 - cvRectangle(m_background, - cvPoint(vpMath::round(rectangle.getLeft() / m_scale), - vpMath::round(rectangle.getBottom() / m_scale)), - cvPoint(vpMath::round(rectangle.getRight() / m_scale), - vpMath::round(rectangle.getTop() / m_scale)), - cvcolor, (int)thickness); + cvRectangle( + m_background, + cvPoint(vpMath::round(rectangle.getLeft() / m_scale), vpMath::round(rectangle.getBottom() / m_scale)), + cvPoint(vpMath::round(rectangle.getRight() / m_scale), vpMath::round(rectangle.getTop() / m_scale)), + cvcolor, (int)thickness); #else cv::rectangle( m_background, - cv::Point(vpMath::round(rectangle.getLeft() / m_scale), - vpMath::round(rectangle.getBottom() / m_scale)), - cv::Point(vpMath::round(rectangle.getRight() / m_scale), - vpMath::round(rectangle.getTop() / m_scale)), + cv::Point(vpMath::round(rectangle.getLeft() / m_scale), vpMath::round(rectangle.getBottom() / m_scale)), + cv::Point(vpMath::round(rectangle.getRight() / m_scale), vpMath::round(rectangle.getTop() / m_scale)), cvcolor, (int)thickness); #endif @@ -1761,44 +1566,37 @@ void vpDisplayOpenCV::displayRectangle(const vpRect &rectangle, #endif if (color.id < vpColor::id_unknown) { #if VISP_HAVE_OPENCV_VERSION < 0x020408 - cvRectangle(m_background, - cvPoint(vpMath::round(rectangle.getLeft() / m_scale), - vpMath::round(rectangle.getBottom() / m_scale)), - cvPoint(vpMath::round(rectangle.getRight() / m_scale), - vpMath::round(rectangle.getTop() / m_scale)), - col[color.id], filled); + cvRectangle( + m_background, + cvPoint(vpMath::round(rectangle.getLeft() / m_scale), vpMath::round(rectangle.getBottom() / m_scale)), + cvPoint(vpMath::round(rectangle.getRight() / m_scale), vpMath::round(rectangle.getTop() / m_scale)), + col[color.id], filled); #else cv::rectangle( m_background, - cv::Point(vpMath::round(rectangle.getLeft() / m_scale), - vpMath::round(rectangle.getBottom() / m_scale)), - cv::Point(vpMath::round(rectangle.getRight() / m_scale), - vpMath::round(rectangle.getTop() / m_scale)), + cv::Point(vpMath::round(rectangle.getLeft() / m_scale), vpMath::round(rectangle.getBottom() / m_scale)), + cv::Point(vpMath::round(rectangle.getRight() / m_scale), vpMath::round(rectangle.getTop() / m_scale)), col[color.id], filled); #endif } else { cvcolor = CV_RGB(color.R, color.G, color.B); #if VISP_HAVE_OPENCV_VERSION < 0x020408 - cvRectangle(m_background, - cvPoint(vpMath::round(rectangle.getLeft() / m_scale), - vpMath::round(rectangle.getBottom() / m_scale)), - cvPoint(vpMath::round(rectangle.getRight() / m_scale), - vpMath::round(rectangle.getTop() / m_scale)), - cvcolor, filled); + cvRectangle( + m_background, + cvPoint(vpMath::round(rectangle.getLeft() / m_scale), vpMath::round(rectangle.getBottom() / m_scale)), + cvPoint(vpMath::round(rectangle.getRight() / m_scale), vpMath::round(rectangle.getTop() / m_scale)), + cvcolor, filled); #else cv::rectangle( m_background, - cv::Point(vpMath::round(rectangle.getLeft() / m_scale), - vpMath::round(rectangle.getBottom() / m_scale)), - cv::Point(vpMath::round(rectangle.getRight() / m_scale), - vpMath::round(rectangle.getTop() / m_scale)), + cv::Point(vpMath::round(rectangle.getLeft() / m_scale), vpMath::round(rectangle.getBottom() / m_scale)), + cv::Point(vpMath::round(rectangle.getRight() / m_scale), vpMath::round(rectangle.getTop() / m_scale)), cvcolor, filled); #endif } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -1848,8 +1646,7 @@ bool vpDisplayOpenCV::getClick(bool blocking) #endif } while (ret == false && blocking == true); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } return ret; } @@ -1918,8 +1715,7 @@ bool vpDisplayOpenCV::getClick(vpImagePoint &ip, bool blocking) #endif } while (ret == false && blocking == true); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } return ret; } @@ -1943,9 +1739,7 @@ bool vpDisplayOpenCV::getClick(vpImagePoint &ip, bool blocking) 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 vpDisplayOpenCV::getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { bool ret = false; @@ -1993,8 +1787,7 @@ bool vpDisplayOpenCV::getClick(vpImagePoint &ip, #endif } while (ret == false && blocking == true); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } return ret; } @@ -2022,9 +1815,7 @@ bool vpDisplayOpenCV::getClick(vpImagePoint &ip, \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool) */ -bool vpDisplayOpenCV::getClickUp(vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, - bool blocking) +bool vpDisplayOpenCV::getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { bool ret = false; if (m_displayHasBeenInitialized) { @@ -2071,8 +1862,7 @@ bool vpDisplayOpenCV::getClickUp(vpImagePoint &ip, #endif } while (ret == false && blocking == true); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } return ret; } @@ -2087,8 +1877,7 @@ void vpDisplayOpenCV::getImage(vpImage &I) // should certainly be optimized. } -void vpDisplayOpenCV::on_mouse(int event, int x, int y, int /*flags*/, - void *display) +void vpDisplayOpenCV::on_mouse(int event, int x, int y, int /*flags*/, void *display) { vpDisplayOpenCV *disp = static_cast(display); switch (event) { @@ -2211,8 +2000,7 @@ bool vpDisplayOpenCV::getKeyboardEvent(bool blocking) return false; return true; } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } // return false; // Never reached after throw() } @@ -2260,8 +2048,7 @@ bool vpDisplayOpenCV::getKeyboardEvent(std::string &key, bool blocking) } return true; } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } // return false; // Never reached after throw() } @@ -2293,8 +2080,7 @@ bool vpDisplayOpenCV::getPointerMotionEvent(vpImagePoint &ip) } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } return ret; } @@ -2322,8 +2108,7 @@ bool vpDisplayOpenCV::getPointerPosition(vpImagePoint &ip) } return false; } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "OpenCV not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized")); } } @@ -2351,11 +2136,9 @@ void vpDisplayOpenCV::getScreenSize(unsigned int &w, unsigned int &h) elm = vpIoTools::splitChain(str, " "); for (size_t i = 0; i < elm.size(); i++) { if (!elm[i].empty()) { - std::vector resolution = - vpIoTools::splitChain(elm[i], "x"); + std::vector resolution = vpIoTools::splitChain(elm[i], "x"); if (resolution.size() == 2) { - std::istringstream sswidth(resolution[0]), - ssheight(resolution[1]); + std::istringstream sswidth(resolution[0]), ssheight(resolution[1]); sswidth >> w; ssheight >> h; break; @@ -2370,9 +2153,8 @@ void vpDisplayOpenCV::getScreenSize(unsigned int &w, unsigned int &h) w = GetSystemMetrics(SM_CXSCREEN); h = GetSystemMetrics(SM_CYSCREEN); #else - throw(vpException(vpException::functionNotImplementedError, - "The function vpDisplayOpenCV::getScreenSize() is not " - "implemented on winrt")); + throw(vpException(vpException::functionNotImplementedError, "The function vpDisplayOpenCV::getScreenSize() is not " + "implemented on winrt")); #endif #endif } diff --git a/modules/gui/src/display/vpDisplayX.cpp b/modules/gui/src/display/vpDisplayX.cpp index 66969bee99..c27bf09a11 100644 --- a/modules/gui/src/display/vpDisplayX.cpp +++ b/modules/gui/src/display/vpDisplayX.cpp @@ -84,10 +84,9 @@ */ vpDisplayX::vpDisplayX(vpImage &I, vpScaleType scaleType) - : 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) + : 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) { setScale(scaleType, I.getWidth(), I.getHeight()); @@ -117,12 +116,10 @@ vpDisplayX::vpDisplayX(vpImage &I, vpScaleType scaleType) and the columns. */ -vpDisplayX::vpDisplayX(vpImage &I, int x, int y, - const std::string &title, vpScaleType scaleType) - : 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) +vpDisplayX::vpDisplayX(vpImage &I, int x, int y, const std::string &title, vpScaleType scaleType) + : 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) { setScale(scaleType, I.getWidth(), I.getHeight()); init(I, x, y, title); @@ -148,10 +145,9 @@ vpDisplayX::vpDisplayX(vpImage &I, int x, int y, and the columns. */ vpDisplayX::vpDisplayX(vpImage &I, vpScaleType scaleType) - : 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) + : 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) { setScale(scaleType, I.getWidth(), I.getHeight()); init(I); @@ -178,12 +174,10 @@ vpDisplayX::vpDisplayX(vpImage &I, vpScaleType scaleType) - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns. */ -vpDisplayX::vpDisplayX(vpImage &I, int x, int y, - const std::string &title, vpScaleType scaleType) - : 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) +vpDisplayX::vpDisplayX(vpImage &I, int x, int y, const std::string &title, vpScaleType scaleType) + : 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) { setScale(scaleType, I.getWidth(), I.getHeight()); init(I, x, y, title); @@ -212,10 +206,9 @@ int main() \endcode */ vpDisplayX::vpDisplayX(int x, int y, const std::string &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) + : 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) { m_windowXPosition = x; m_windowYPosition = y; @@ -243,10 +236,9 @@ int main() \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) + : 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) { } @@ -263,8 +255,7 @@ vpDisplayX::~vpDisplayX() { closeDisplay(); } \param title : Window title. */ -void vpDisplayX::init(vpImage &I, int x, int y, - const std::string &title) +void vpDisplayX::init(vpImage &I, int x, int y, const std::string &title) { setScale(m_scaleType, I.getWidth(), I.getHeight()); @@ -296,31 +287,25 @@ void vpDisplayX::init(vpImage &I, int x, int y, m_height = I.getHeight() / m_scale; 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.")); + 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), m_windowXPosition, - m_windowYPosition, m_width, m_height, 1, - BlackPixel(display, screen), WhitePixel(display, screen))) == 0) { + if ((window = XCreateSimpleWindow(display, RootWindow(display, screen), m_windowXPosition, m_windowYPosition, m_width, + m_height, 1, BlackPixel(display, screen), WhitePixel(display, screen))) == 0) { vpERROR_TRACE("Can't create window."); - throw(vpDisplayException(vpDisplayException::cannotOpenWindowError, - "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); + lut = XCreateColormap(display, window, DefaultVisual(display, screen), AllocAll); xcolor.flags = DoRed | DoGreen | DoBlue; for (unsigned int i = 0; i < 256; i++) { @@ -341,8 +326,7 @@ void vpDisplayX::init(vpImage &I, int x, int y, xcolor.red = xcolor.green = xcolor.blue = 256 * i; if (XAllocColor(display, lut, &xcolor) == 0) { vpERROR_TRACE("Can't allocate 256 colors. Only %d allocated.", i); - throw(vpDisplayException(vpDisplayException::colorAllocError, - "Can't allocate 256 colors.")); + throw(vpDisplayException(vpDisplayException::colorAllocError, "Can't allocate 256 colors.")); } colortable[i] = xcolor.pixel; } @@ -648,28 +632,23 @@ void vpDisplayX::init(vpImage &I, int x, int y, } } - XSetStandardProperties(display, window, this->m_title.c_str(), - this->m_title.c_str(), None, 0, 0, &hints); + XSetStandardProperties(display, window, this->m_title.c_str(), this->m_title.c_str(), None, 0, 0, &hints); XMapWindow(display, window); // Selection des evenements. XSelectInput(display, window, - ExposureMask | ButtonPressMask | ButtonReleaseMask | - KeyPressMask | KeyReleaseMask | StructureNotifyMask | - PointerMotionMask); + 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); + 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")); + throw(vpDisplayException(vpDisplayException::XWindowsError, "Can't create graphics context")); } // Pixmap creation. @@ -681,12 +660,10 @@ void vpDisplayX::init(vpImage &I, int x, int y, // while ( event.xany.type != Expose ); { - Ximage = XCreateImage(display, DefaultVisual(display, screen), - screen_depth, ZPixmap, 0, NULL, m_width, m_height, + Ximage = XCreateImage(display, DefaultVisual(display, screen), screen_depth, ZPixmap, 0, NULL, m_width, m_height, XBitmapPad(display), 0); - Ximage->data = - (char *)malloc(m_height * (unsigned int)Ximage->bytes_per_line); + Ximage->data = (char *)malloc(m_height * (unsigned int)Ximage->bytes_per_line); ximage_data_init = true; } m_displayHasBeenInitialized = true; @@ -707,8 +684,7 @@ void vpDisplayX::init(vpImage &I, int x, int y, \param title : Window title. */ -void vpDisplayX::init(vpImage &I, int x, int y, - const std::string &title) +void vpDisplayX::init(vpImage &I, int x, int y, const std::string &title) { setScale(m_scaleType, I.getWidth(), I.getHeight()); @@ -740,10 +716,8 @@ void vpDisplayX::init(vpImage &I, int x, int y, m_height = I.getHeight() / m_scale; 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.")); + 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); @@ -752,21 +726,17 @@ void vpDisplayX::init(vpImage &I, int x, int y, vpDEBUG_TRACE(1, "Screen depth: %d\n", screen_depth); - if ((window = XCreateSimpleWindow( - display, RootWindow(display, screen), m_windowXPosition, - m_windowYPosition, m_width, m_height, 1, - BlackPixel(display, screen), WhitePixel(display, screen))) == 0) { + if ((window = XCreateSimpleWindow(display, RootWindow(display, screen), m_windowXPosition, m_windowYPosition, m_width, + m_height, 1, BlackPixel(display, screen), WhitePixel(display, screen))) == 0) { vpERROR_TRACE("Can't create window."); - throw(vpDisplayException(vpDisplayException::cannotOpenWindowError, - "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); + lut = XCreateColormap(display, window, DefaultVisual(display, screen), AllocAll); xcolor.flags = DoRed | DoGreen | DoBlue; for (unsigned int i = 0; i < 256; i++) { @@ -787,8 +757,7 @@ void vpDisplayX::init(vpImage &I, int x, int y, xcolor.red = xcolor.green = xcolor.blue = 256 * i; if (XAllocColor(display, lut, &xcolor) == 0) { vpERROR_TRACE("Can't allocate 256 colors. Only %d allocated.", i); - throw(vpDisplayException(vpDisplayException::colorAllocError, - "Can't allocate 256 colors.")); + throw(vpDisplayException(vpDisplayException::colorAllocError, "Can't allocate 256 colors.")); } colortable[i] = xcolor.pixel; } @@ -1095,28 +1064,23 @@ void vpDisplayX::init(vpImage &I, int x, int y, } } - XSetStandardProperties(display, window, this->m_title.c_str(), - this->m_title.c_str(), None, 0, 0, &hints); + XSetStandardProperties(display, window, this->m_title.c_str(), this->m_title.c_str(), None, 0, 0, &hints); XMapWindow(display, window); // Selection des evenements. XSelectInput(display, window, - ExposureMask | ButtonPressMask | ButtonReleaseMask | - KeyPressMask | KeyReleaseMask | StructureNotifyMask | - PointerMotionMask); + 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); + 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")); + throw(vpDisplayException(vpDisplayException::XWindowsError, "Can't create graphics context")); } // Pixmap creation. @@ -1128,12 +1092,10 @@ void vpDisplayX::init(vpImage &I, int x, int y, // while ( event.xany.type != Expose ); { - Ximage = XCreateImage(display, DefaultVisual(display, screen), - screen_depth, ZPixmap, 0, NULL, m_width, m_height, + Ximage = XCreateImage(display, DefaultVisual(display, screen), screen_depth, ZPixmap, 0, NULL, m_width, m_height, XBitmapPad(display), 0); - Ximage->data = - (char *)malloc(m_height * (unsigned int)Ximage->bytes_per_line); + Ximage->data = (char *)malloc(m_height * (unsigned int)Ximage->bytes_per_line); ximage_data_init = true; } m_displayHasBeenInitialized = true; @@ -1152,8 +1114,7 @@ void vpDisplayX::init(vpImage &I, int x, int y, \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 std::string &title) +void vpDisplayX::init(unsigned int w, unsigned int h, int x, int y, const std::string &title) { setScale(m_scaleType, w, h); @@ -1184,10 +1145,8 @@ void vpDisplayX::init(unsigned int w, unsigned int h, int x, int y, m_title = title; 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.")); + 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); @@ -1196,21 +1155,17 @@ void vpDisplayX::init(unsigned int w, unsigned int h, int x, int y, vpTRACE("Screen depth: %d\n", screen_depth); - if ((window = XCreateSimpleWindow( - display, RootWindow(display, screen), m_windowXPosition, - m_windowYPosition, m_width, m_height, 1, - BlackPixel(display, screen), WhitePixel(display, screen))) == 0) { + if ((window = XCreateSimpleWindow(display, RootWindow(display, screen), m_windowXPosition, m_windowYPosition, m_width, + m_height, 1, BlackPixel(display, screen), WhitePixel(display, screen))) == 0) { vpERROR_TRACE("Can't create window."); - throw(vpDisplayException(vpDisplayException::cannotOpenWindowError, - "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); + lut = XCreateColormap(display, window, DefaultVisual(display, screen), AllocAll); xcolor.flags = DoRed | DoGreen | DoBlue; for (unsigned int i = 0; i < 256; i++) { @@ -1231,8 +1186,7 @@ void vpDisplayX::init(unsigned int w, unsigned int h, int x, int y, xcolor.red = xcolor.green = xcolor.blue = 256 * i; if (XAllocColor(display, lut, &xcolor) == 0) { vpERROR_TRACE("Can't allocate 256 colors. Only %d allocated.", i); - throw(vpDisplayException(vpDisplayException::colorAllocError, - "Can't allocate 256 colors.")); + throw(vpDisplayException(vpDisplayException::colorAllocError, "Can't allocate 256 colors.")); } colortable[i] = xcolor.pixel; } @@ -1561,28 +1515,23 @@ void vpDisplayX::init(unsigned int w, unsigned int h, int x, int y, } } - XSetStandardProperties(display, window, this->m_title.c_str(), - this->m_title.c_str(), None, 0, 0, &hints); + XSetStandardProperties(display, window, this->m_title.c_str(), this->m_title.c_str(), None, 0, 0, &hints); XMapWindow(display, window); // Selection des evenements. XSelectInput(display, window, - ExposureMask | ButtonPressMask | ButtonReleaseMask | - KeyPressMask | KeyReleaseMask | StructureNotifyMask | - PointerMotionMask); + 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); + 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")); + throw(vpDisplayException(vpDisplayException::XWindowsError, "Can't create graphics context")); } // Pixmap creation. @@ -1594,12 +1543,10 @@ void vpDisplayX::init(unsigned int w, unsigned int h, int x, int y, // while ( event.xany.type != Expose ); { - Ximage = XCreateImage(display, DefaultVisual(display, screen), - screen_depth, ZPixmap, 0, NULL, m_width, m_height, + Ximage = XCreateImage(display, DefaultVisual(display, screen), screen_depth, ZPixmap, 0, NULL, m_width, m_height, XBitmapPad(display), 0); - Ximage->data = - (char *)malloc(m_height * (unsigned int)Ximage->bytes_per_line); + Ximage->data = (char *)malloc(m_height * (unsigned int)Ximage->bytes_per_line); ximage_data_init = true; } m_displayHasBeenInitialized = true; @@ -1629,17 +1576,14 @@ void vpDisplayX::setFont(const std::string &font) if (!font.empty()) { try { Font stringfont; - stringfont = XLoadFont( - display, font.c_str()); //"-adobe-times-bold-r-normal--18*"); + stringfont = XLoadFont(display, font.c_str()); //"-adobe-times-bold-r-normal--18*"); XSetFont(display, context, stringfont); } catch (...) { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "Bad font")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "Bad font")); } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -1654,8 +1598,7 @@ void vpDisplayX::setTitle(const std::string &title) if (!title.empty()) XStoreName(display, window, m_title.c_str()); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -1673,8 +1616,7 @@ void vpDisplayX::setWindowPosition(int winx, int winy) if (m_displayHasBeenInitialized) { XMoveWindow(display, window, winx, winy); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -1728,8 +1670,7 @@ void vpDisplayX::displayImage(const vpImage &I) } // Affichage de l'image dans la Pixmap. - XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, - m_height); + XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, m_height); XSetWindowBackgroundPixmap(display, window, pixmap); break; } @@ -1737,8 +1678,7 @@ void vpDisplayX::displayImage(const vpImage &I) unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line; if (m_scale == 1) { for (unsigned int i = 0; i < m_height; i++) { - unsigned char *dst_8 = - (unsigned char *)Ximage->data + i * bytes_per_line; + unsigned char *dst_8 = (unsigned char *)Ximage->data + i * bytes_per_line; unsigned short *dst_16 = (unsigned short *)dst_8; for (unsigned int j = 0; j < m_width; j++) { *(dst_16 + j) = (unsigned short)colortable[I[i][j]]; @@ -1746,19 +1686,16 @@ void vpDisplayX::displayImage(const vpImage &I) } } else { for (unsigned int i = 0; i < m_height; i++) { - unsigned char *dst_8 = - (unsigned char *)Ximage->data + i * bytes_per_line; + unsigned char *dst_8 = (unsigned char *)Ximage->data + i * bytes_per_line; unsigned short *dst_16 = (unsigned short *)dst_8; for (unsigned int j = 0; j < m_width; j++) { - *(dst_16 + j) = - (unsigned short)colortable[I[i * m_scale][j * m_scale]]; + *(dst_16 + j) = (unsigned short)colortable[I[i * m_scale][j * m_scale]]; } } } // Affichage de l'image dans la Pixmap. - XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, - m_height); + XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, m_height); XSetWindowBackgroundPixmap(display, window, pixmap); break; } @@ -1818,15 +1755,13 @@ void vpDisplayX::displayImage(const vpImage &I) } // Affichage de l'image dans la Pixmap. - XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, - m_height); + XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, m_height); XSetWindowBackgroundPixmap(display, window, pixmap); break; } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } /*! @@ -1851,39 +1786,34 @@ void vpDisplayX::displayImage(const vpImage &I) if (m_scale == 1) { for (unsigned int i = 0; i < m_height; i++) { - unsigned char *dst_8 = - (unsigned char *)Ximage->data + i * bytes_per_line; + unsigned char *dst_8 = (unsigned char *)Ximage->data + i * bytes_per_line; unsigned short *dst_16 = (unsigned short *)dst_8; for (unsigned int j = 0; j < m_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); + *(dst_16 + j) = + (((r << 8) >> RShift) & RMask) | (((g << 8) >> GShift) & GMask) | (((b << 8) >> BShift) & BMask); bitmap++; } } } else { for (unsigned int i = 0; i < m_height; i++) { - unsigned char *dst_8 = - (unsigned char *)Ximage->data + i * bytes_per_line; + unsigned char *dst_8 = (unsigned char *)Ximage->data + i * bytes_per_line; unsigned short *dst_16 = (unsigned short *)dst_8; for (unsigned int j = 0; j < m_width; j++) { vpRGBa val = I[i * m_scale][j * m_scale]; r = val.R; g = val.G; b = val.B; - *(dst_16 + j) = (((r << 8) >> RShift) & RMask) | - (((g << 8) >> GShift) & GMask) | - (((b << 8) >> BShift) & BMask); + *(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, m_width, - m_height); + XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, m_height); XSetWindowBackgroundPixmap(display, window, pixmap); break; @@ -1944,19 +1874,16 @@ void vpDisplayX::displayImage(const vpImage &I) } // Affichage de l'image dans la Pixmap. - XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, - m_height); + XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, m_height); XSetWindowBackgroundPixmap(display, window, pixmap); break; } default: throw(vpDisplayException(vpDisplayException::depthNotSupportedError, - "Unsupported depth (%d bpp) for color display", - screen_depth)); + "Unsupported depth (%d bpp) for color display", screen_depth)); } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -1985,12 +1912,10 @@ void vpDisplayX::displayImage(const unsigned char *bitmap) } // Affichage de l'image dans la Pixmap. - XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, - m_height); + XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, m_height); XSetWindowBackgroundPixmap(display, window, pixmap); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2009,8 +1934,7 @@ void vpDisplayX::displayImage(const unsigned char *bitmap) \sa init(), closeDisplay() */ -void vpDisplayX::displayImageROI(const vpImage &I, - const vpImagePoint &iP, const unsigned int w, +void vpDisplayX::displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int w, const unsigned int h) { if (m_displayHasBeenInitialized) { @@ -2043,17 +1967,15 @@ void vpDisplayX::displayImageROI(const vpImage &I, i++; } - XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), - (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h); + XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), + w, h); } else { // Correction de l'image de facon a liberer les niveaux de gris // ROUGE, VERT, BLEU, JAUNE int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = - (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = - (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); unsigned int i_min_ = (unsigned int)i_min; unsigned int i_max_ = (unsigned int)i_max; @@ -2070,8 +1992,7 @@ void vpDisplayX::displayImageROI(const vpImage &I, dst_8[j] = nivGris; } } - XPutImage(display, pixmap, context, Ximage, j_min, i_min, j_min, - i_min, j_max_ - j_min_, i_max_ - i_min_); + XPutImage(display, pixmap, context, Ximage, j_min, i_min, j_min, i_min, j_max_ - j_min_, i_max_ - i_min_); } // Affichage de l'image dans la Pixmap. @@ -2081,26 +2002,21 @@ void vpDisplayX::displayImageROI(const vpImage &I, case 16: { unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line; if (m_scale == 1) { - for (unsigned int i = (unsigned int)iP.get_i(); - i < (unsigned int)(iP.get_i() + h); i++) { - unsigned char *dst_8 = - (unsigned char *)Ximage->data + i * bytes_per_line; + for (unsigned int i = (unsigned int)iP.get_i(); i < (unsigned int)(iP.get_i() + h); i++) { + unsigned char *dst_8 = (unsigned char *)Ximage->data + i * bytes_per_line; unsigned short *dst_16 = (unsigned short *)dst_8; - for (unsigned int j = (unsigned int)iP.get_j(); - j < (unsigned int)(iP.get_j() + w); j++) { + 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]]; } } - XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), - (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h); + XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), + w, h); } else { int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = - (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = - (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); unsigned int i_min_ = (unsigned int)i_min; unsigned int i_max_ = (unsigned int)i_max; @@ -2108,17 +2024,14 @@ void vpDisplayX::displayImageROI(const vpImage &I, unsigned int j_max_ = (unsigned int)j_max; for (unsigned int i = i_min_; i < i_max_; i++) { - unsigned char *dst_8 = - (unsigned char *)Ximage->data + i * bytes_per_line; + unsigned char *dst_8 = (unsigned char *)Ximage->data + i * bytes_per_line; unsigned short *dst_16 = (unsigned short *)dst_8; for (unsigned int j = j_min_; j < j_max_; j++) { - *(dst_16 + j) = - (unsigned short)colortable[I[i * m_scale][j * m_scale]]; + *(dst_16 + j) = (unsigned short)colortable[I[i * m_scale][j * m_scale]]; } } - XPutImage(display, pixmap, context, Ximage, j_min, i_min, j_min, - i_min, j_max_ - j_min_, i_max_ - i_min_); + XPutImage(display, pixmap, context, Ximage, j_min, i_min, j_min, i_min, j_max_ - j_min_, i_max_ - i_min_); } XSetWindowBackgroundPixmap(display, window, pixmap); @@ -2129,11 +2042,8 @@ void vpDisplayX::displayImageROI(const vpImage &I, default: { if (m_scale == 1) { unsigned int iwidth = I.getWidth(); - unsigned char *src_8 = - I.bitmap + (int)(iP.get_i() * iwidth + iP.get_j()); - unsigned char *dst_32 = - (unsigned char *)Ximage->data + - (int)(iP.get_i() * 4 * m_width + iP.get_j() * 4); + unsigned char *src_8 = I.bitmap + (int)(iP.get_i() * iwidth + iP.get_j()); + unsigned char *dst_32 = (unsigned char *)Ximage->data + (int)(iP.get_i() * 4 * m_width + iP.get_j() * 4); if (XImageByteOrder(display) == 1) { // big endian @@ -2171,15 +2081,13 @@ void vpDisplayX::displayImageROI(const vpImage &I, } } - XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), - (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h); + XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), + w, h); } else { int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = - (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = - (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); unsigned int i_min_ = (unsigned int)i_min; unsigned int i_max_ = (unsigned int)i_max; @@ -2189,8 +2097,7 @@ void vpDisplayX::displayImageROI(const vpImage &I, if (XImageByteOrder(display) == 1) { // big endian for (unsigned int i = i_min_; i < i_max_; i++) { - unsigned char *dst_32 = (unsigned char *)Ximage->data + - (int)(i * 4 * m_width + j_min_ * 4); + unsigned char *dst_32 = (unsigned char *)Ximage->data + (int)(i * 4 * m_width + j_min_ * 4); for (unsigned int j = j_min_; j < j_max_; j++) { unsigned char val = I[i * m_scale][j * m_scale]; *(dst_32++) = vpRGBa::alpha_default; @@ -2202,8 +2109,7 @@ void vpDisplayX::displayImageROI(const vpImage &I, } else { // little endian for (unsigned int i = i_min_; i < i_max_; i++) { - unsigned char *dst_32 = (unsigned char *)Ximage->data + - (int)(i * 4 * m_width + j_min_ * 4); + unsigned char *dst_32 = (unsigned char *)Ximage->data + (int)(i * 4 * m_width + j_min_ * 4); for (unsigned int j = j_min_; j < j_max_; j++) { unsigned char val = I[i * m_scale][j * m_scale]; *(dst_32++) = val; @@ -2214,8 +2120,7 @@ void vpDisplayX::displayImageROI(const vpImage &I, } } - XPutImage(display, pixmap, context, Ximage, j_min, i_min, j_min, - i_min, j_max_ - j_min_, i_max_ - i_min_); + XPutImage(display, pixmap, context, Ximage, j_min, i_min, j_min, i_min, j_max_ - j_min_, i_max_ - i_min_); } XSetWindowBackgroundPixmap(display, window, pixmap); @@ -2223,8 +2128,7 @@ void vpDisplayX::displayImageROI(const vpImage &I, } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2243,8 +2147,7 @@ void vpDisplayX::displayImageROI(const vpImage &I, \sa init(), closeDisplay() */ -void vpDisplayX::displayImageROI(const vpImage &I, - const vpImagePoint &iP, const unsigned int w, +void vpDisplayX::displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int w, const unsigned int h) { if (m_displayHasBeenInitialized) { @@ -2252,32 +2155,26 @@ void vpDisplayX::displayImageROI(const vpImage &I, case 16: { if (m_scale == 1) { 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++) { - unsigned char *dst_8 = - (unsigned char *)Ximage->data + i * bytes_per_line; + for (unsigned int i = (unsigned int)iP.get_i(); i < (unsigned int)(iP.get_i() + h); i++) { + unsigned char *dst_8 = (unsigned char *)Ximage->data + i * bytes_per_line; unsigned short *dst_16 = (unsigned short *)dst_8; - for (unsigned int j = (unsigned int)iP.get_j(); - j < (unsigned int)(iP.get_j() + w); j++) { + for (unsigned int j = (unsigned int)iP.get_j(); j < (unsigned int)(iP.get_j() + w); j++) { vpRGBa val = I[i][j]; unsigned int r = val.R; unsigned int g = val.G; unsigned int b = val.B; - *(dst_16 + j) = (((r << 8) >> RShift) & RMask) | - (((g << 8) >> GShift) & GMask) | - (((b << 8) >> BShift) & BMask); + *(dst_16 + j) = + (((r << 8) >> RShift) & RMask) | (((g << 8) >> GShift) & GMask) | (((b << 8) >> BShift) & BMask); } } - XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), - (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h); + XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), + w, h); } else { unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line; int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = - (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = - (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); unsigned int i_min_ = (unsigned int)i_min; unsigned int i_max_ = (unsigned int)i_max; @@ -2285,21 +2182,18 @@ void vpDisplayX::displayImageROI(const vpImage &I, unsigned int j_max_ = (unsigned int)j_max; for (unsigned int i = i_min_; i < i_max_; i++) { - unsigned char *dst_8 = - (unsigned char *)Ximage->data + i * bytes_per_line; + unsigned char *dst_8 = (unsigned char *)Ximage->data + i * bytes_per_line; unsigned short *dst_16 = (unsigned short *)dst_8; for (unsigned int j = j_min_; j < j_max_; j++) { vpRGBa val = I[i * m_scale][j * m_scale]; unsigned int r = val.R; unsigned int g = val.G; unsigned int b = val.B; - *(dst_16 + j) = (((r << 8) >> RShift) & RMask) | - (((g << 8) >> GShift) & GMask) | - (((b << 8) >> BShift) & BMask); + *(dst_16 + j) = + (((r << 8) >> RShift) & RMask) | (((g << 8) >> GShift) & GMask) | (((b << 8) >> BShift) & BMask); } } - XPutImage(display, pixmap, context, Ximage, j_min, i_min, j_min, - i_min, j_max_ - j_min_, i_max_ - i_min_); + XPutImage(display, pixmap, context, Ximage, j_min, i_min, j_min, i_min, j_max_ - j_min_, i_max_ - i_min_); } XSetWindowBackgroundPixmap(display, window, pixmap); @@ -2358,15 +2252,13 @@ void vpDisplayX::displayImageROI(const vpImage &I, } } - XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), - (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h); + XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), + w, h); } else { int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = - (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = - (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); unsigned int i_min_ = (unsigned int)i_min; unsigned int i_max_ = (unsigned int)i_max; @@ -2376,8 +2268,7 @@ void vpDisplayX::displayImageROI(const vpImage &I, if (XImageByteOrder(display) == 1) { // big endian for (unsigned int i = i_min_; i < i_max_; i++) { - unsigned char *dst_32 = (unsigned char *)Ximage->data + - (int)(i * 4 * m_width + j_min_ * 4); + unsigned char *dst_32 = (unsigned char *)Ximage->data + (int)(i * 4 * m_width + j_min_ * 4); for (unsigned int j = j_min_; j < j_max_; j++) { vpRGBa val = I[i * m_scale][j * m_scale]; *(dst_32++) = val.A; @@ -2389,8 +2280,7 @@ void vpDisplayX::displayImageROI(const vpImage &I, } else { // little endian for (unsigned int i = i_min_; i < i_max_; i++) { - unsigned char *dst_32 = (unsigned char *)Ximage->data + - (int)(i * 4 * m_width + j_min_ * 4); + unsigned char *dst_32 = (unsigned char *)Ximage->data + (int)(i * 4 * m_width + j_min_ * 4); for (unsigned int j = j_min_; j < j_max_; j++) { vpRGBa val = I[i * m_scale][j * m_scale]; *(dst_32++) = val.B; @@ -2400,8 +2290,7 @@ void vpDisplayX::displayImageROI(const vpImage &I, } } } - XPutImage(display, pixmap, context, Ximage, j_min, i_min, j_min, - i_min, j_max_ - j_min_, i_max_ - i_min_); + XPutImage(display, pixmap, context, Ximage, j_min, i_min, j_min, i_min, j_max_ - j_min_, i_max_ - i_min_); } XSetWindowBackgroundPixmap(display, window, pixmap); @@ -2409,12 +2298,10 @@ void vpDisplayX::displayImageROI(const vpImage &I, } default: throw(vpDisplayException(vpDisplayException::depthNotSupportedError, - "Unsupported depth (%d bpp) for color display", - screen_depth)); + "Unsupported depth (%d bpp) for color display", screen_depth)); } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2460,8 +2347,7 @@ void vpDisplayX::flushDisplay() XClearWindow(display, window); XFlush(display); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2472,16 +2358,13 @@ void vpDisplayX::flushDisplay() \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) +void vpDisplayX::flushDisplayROI(const vpImagePoint &iP, const unsigned int w, const unsigned int h) { if (m_displayHasBeenInitialized) { - XClearArea(display, window, (int)(iP.get_u() / m_scale), - (int)(iP.get_v() / m_scale), w / m_scale, h / m_scale, 0); + XClearArea(display, window, (int)(iP.get_u() / m_scale), (int)(iP.get_v() / m_scale), w / m_scale, h / m_scale, 0); XFlush(display); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2510,8 +2393,7 @@ void vpDisplayX::clearDisplay(const vpColor &color) // Pixmap creation. pixmap = XCreatePixmap(display, window, m_width, m_height, screen_depth); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2522,10 +2404,8 @@ void vpDisplayX::clearDisplay(const vpColor &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) +void vpDisplayX::displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int w, + unsigned int h, unsigned int thickness) { if (m_displayHasBeenInitialized) { double a = ip2.get_i() - ip1.get_i(); @@ -2560,8 +2440,7 @@ void vpDisplayX::displayArrow(const vpImagePoint &ip1, displayLine(ip1, ip2, color, thickness); } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2576,8 +2455,7 @@ void vpDisplayX::displayArrow(const vpImagePoint &ip1, \sa setFont() */ -void vpDisplayX::displayCharString(const vpImagePoint &ip, const char *text, - const vpColor &color) +void vpDisplayX::displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color) { if (m_displayHasBeenInitialized) { if (color.id < vpColor::id_unknown) @@ -2590,11 +2468,10 @@ void vpDisplayX::displayCharString(const vpImagePoint &ip, const char *text, XAllocColor(display, lut, &xcolor); XSetForeground(display, context, xcolor.pixel); } - XDrawString(display, pixmap, context, (int)(ip.get_u() / m_scale), - (int)(ip.get_v() / m_scale), text, (int)strlen(text)); + XDrawString(display, pixmap, context, (int)(ip.get_u() / m_scale), (int)(ip.get_v() / m_scale), text, + (int)strlen(text)); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2607,9 +2484,8 @@ void vpDisplayX::displayCharString(const vpImagePoint &ip, const char *text, \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) +void vpDisplayX::displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill, + unsigned int thickness) { if (m_displayHasBeenInitialized) { if (thickness == 1) @@ -2625,25 +2501,19 @@ void vpDisplayX::displayCircle(const vpImagePoint ¢er, XSetForeground(display, context, xcolor.pixel); } - XSetLineAttributes(display, context, thickness, LineSolid, CapButt, - JoinBevel); + XSetLineAttributes(display, context, thickness, LineSolid, CapButt, JoinBevel); if (fill == false) { - XDrawArc(display, pixmap, context, - vpMath::round((center.get_u() - radius) / m_scale), - vpMath::round((center.get_v() - radius) / m_scale), - radius * 2 / m_scale, radius * 2 / m_scale, 0, + XDrawArc(display, pixmap, context, vpMath::round((center.get_u() - radius) / m_scale), + vpMath::round((center.get_v() - radius) / m_scale), radius * 2 / m_scale, radius * 2 / m_scale, 0, 23040); /* 23040 = 360*64 */ } else { - XFillArc(display, pixmap, context, - vpMath::round((center.get_u() - radius) / m_scale), - vpMath::round((center.get_v() - radius) / m_scale), - radius * 2 / m_scale, radius * 2 / m_scale, 0, + XFillArc(display, pixmap, context, vpMath::round((center.get_u() - radius) / m_scale), + vpMath::round((center.get_v() - radius) / m_scale), radius * 2 / m_scale, radius * 2 / m_scale, 0, 23040); /* 23040 = 360*64 */ } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2654,8 +2524,8 @@ void vpDisplayX::displayCircle(const vpImagePoint ¢er, \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) +void vpDisplayX::displayCross(const vpImagePoint &ip, unsigned int cross_size, const vpColor &color, + unsigned int thickness) { if (m_displayHasBeenInitialized) { double i = ip.get_i(); @@ -2675,8 +2545,7 @@ void vpDisplayX::displayCross(const vpImagePoint &ip, unsigned int cross_size, displayLine(ip1, ip2, color, thickness); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } /*! @@ -2685,8 +2554,7 @@ void vpDisplayX::displayCross(const vpImagePoint &ip, unsigned int cross_size, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplayX::displayDotLine(const vpImagePoint &ip1, - const vpImagePoint &ip2, const vpColor &color, +void vpDisplayX::displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness) { if (m_displayHasBeenInitialized) { @@ -2704,16 +2572,12 @@ void vpDisplayX::displayDotLine(const vpImagePoint &ip1, XSetForeground(display, context, xcolor.pixel); } - XSetLineAttributes(display, context, thickness, LineOnOffDash, CapButt, - JoinBevel); + XSetLineAttributes(display, context, thickness, LineOnOffDash, CapButt, JoinBevel); - XDrawLine(display, pixmap, context, vpMath::round(ip1.get_u() / m_scale), - vpMath::round(ip1.get_v() / m_scale), - vpMath::round(ip2.get_u() / m_scale), - vpMath::round(ip2.get_v() / m_scale)); + XDrawLine(display, pixmap, context, vpMath::round(ip1.get_u() / m_scale), vpMath::round(ip1.get_v() / m_scale), + vpMath::round(ip2.get_u() / m_scale), vpMath::round(ip2.get_v() / m_scale)); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2723,8 +2587,8 @@ void vpDisplayX::displayDotLine(const vpImagePoint &ip1, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplayX::displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, - const vpColor &color, unsigned int thickness) +void vpDisplayX::displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, + unsigned int thickness) { if (m_displayHasBeenInitialized) { if (thickness == 1) @@ -2741,16 +2605,12 @@ void vpDisplayX::displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, XSetForeground(display, context, xcolor.pixel); } - XSetLineAttributes(display, context, thickness, LineSolid, CapButt, - JoinBevel); + XSetLineAttributes(display, context, thickness, LineSolid, CapButt, JoinBevel); - XDrawLine(display, pixmap, context, vpMath::round(ip1.get_u() / m_scale), - vpMath::round(ip1.get_v() / m_scale), - vpMath::round(ip2.get_u() / m_scale), - vpMath::round(ip2.get_v() / m_scale)); + XDrawLine(display, pixmap, context, vpMath::round(ip1.get_u() / m_scale), vpMath::round(ip1.get_v() / m_scale), + vpMath::round(ip2.get_u() / m_scale), vpMath::round(ip2.get_v() / m_scale)); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2760,8 +2620,7 @@ void vpDisplayX::displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, \param color : Point color. \param thickness : Point thickness. */ -void vpDisplayX::displayPoint(const vpImagePoint &ip, const vpColor &color, - unsigned int thickness) +void vpDisplayX::displayPoint(const vpImagePoint &ip, const vpColor &color, unsigned int thickness) { if (m_displayHasBeenInitialized) { if (color.id < vpColor::id_unknown) @@ -2776,18 +2635,14 @@ void vpDisplayX::displayPoint(const vpImagePoint &ip, const vpColor &color, } if (thickness == 1) { - XDrawPoint(display, pixmap, context, - vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale)); + XDrawPoint(display, pixmap, context, vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale)); } else { - XFillRectangle( - display, pixmap, context, vpMath::round(ip.get_u() / m_scale), - vpMath::round(ip.get_v() / m_scale), thickness, thickness); + XFillRectangle(display, pixmap, context, vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale), + thickness, thickness); } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2804,8 +2659,7 @@ void vpDisplayX::displayPoint(const vpImagePoint &ip, const vpColor &color, 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, +void vpDisplayX::displayRectangle(const vpImagePoint &topLeft, unsigned int w, unsigned int h, const vpColor &color, bool fill, unsigned int thickness) { if (m_displayHasBeenInitialized) { @@ -2821,20 +2675,16 @@ void vpDisplayX::displayRectangle(const vpImagePoint &topLeft, unsigned int w, XAllocColor(display, lut, &xcolor); XSetForeground(display, context, xcolor.pixel); } - XSetLineAttributes(display, context, thickness, LineSolid, CapButt, - JoinBevel); + XSetLineAttributes(display, context, thickness, LineSolid, CapButt, JoinBevel); if (fill == false) { - XDrawRectangle( - display, pixmap, context, vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale), w / m_scale, h / m_scale); + XDrawRectangle(display, pixmap, context, vpMath::round(topLeft.get_u() / m_scale), + vpMath::round(topLeft.get_v() / m_scale), w / m_scale, h / m_scale); } else { - XFillRectangle( - display, pixmap, context, vpMath::round(topLeft.get_u() / m_scale), - vpMath::round(topLeft.get_v() / m_scale), w / m_scale, h / m_scale); + XFillRectangle(display, pixmap, context, vpMath::round(topLeft.get_u() / m_scale), + vpMath::round(topLeft.get_v() / m_scale), w / m_scale, h / m_scale); } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2850,10 +2700,8 @@ void vpDisplayX::displayRectangle(const vpImagePoint &topLeft, unsigned int w, 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) +void vpDisplayX::displayRectangle(const vpImagePoint &topLeft, const vpImagePoint &bottomRight, const vpColor &color, + bool fill, unsigned int thickness) { if (m_displayHasBeenInitialized) { if (thickness == 1) @@ -2869,38 +2717,26 @@ void vpDisplayX::displayRectangle(const vpImagePoint &topLeft, XSetForeground(display, context, xcolor.pixel); } - XSetLineAttributes(display, context, thickness, LineSolid, CapButt, - JoinBevel); + XSetLineAttributes(display, context, thickness, LineSolid, CapButt, JoinBevel); vpImagePoint topLeft_ = topLeft / m_scale; vpImagePoint bottomRight_ = bottomRight / m_scale; - 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())); + 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()), + 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, 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()), + 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 { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2916,9 +2752,7 @@ void vpDisplayX::displayRectangle(const vpImagePoint &topLeft, false. */ -void vpDisplayX::displayRectangle(const vpRect &rectangle, - const vpColor &color, bool fill, - unsigned int thickness) +void vpDisplayX::displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill, unsigned int thickness) { if (m_displayHasBeenInitialized) { if (thickness == 1) @@ -2934,28 +2768,22 @@ void vpDisplayX::displayRectangle(const vpRect &rectangle, XSetForeground(display, context, xcolor.pixel); } - XSetLineAttributes(display, context, thickness, LineSolid, CapButt, - JoinBevel); + XSetLineAttributes(display, context, thickness, LineSolid, CapButt, JoinBevel); if (fill == false) { - XDrawRectangle( - display, pixmap, context, - vpMath::round(rectangle.getLeft() / m_scale), - vpMath::round(rectangle.getTop() / m_scale), - (unsigned int)vpMath::round(rectangle.getWidth() / m_scale - 1), - (unsigned int)vpMath::round(rectangle.getHeight() / m_scale - 1)); + XDrawRectangle(display, pixmap, context, vpMath::round(rectangle.getLeft() / m_scale), + vpMath::round(rectangle.getTop() / m_scale), + (unsigned int)vpMath::round(rectangle.getWidth() / m_scale - 1), + (unsigned int)vpMath::round(rectangle.getHeight() / m_scale - 1)); } else { - XFillRectangle( - display, pixmap, context, - vpMath::round(rectangle.getLeft() / m_scale), - vpMath::round(rectangle.getTop() / m_scale), - (unsigned int)vpMath::round(rectangle.getWidth() / m_scale), - (unsigned int)vpMath::round(rectangle.getHeight() / m_scale)); + XFillRectangle(display, pixmap, context, vpMath::round(rectangle.getLeft() / m_scale), + vpMath::round(rectangle.getTop() / m_scale), + (unsigned int)vpMath::round(rectangle.getWidth() / m_scale), + (unsigned int)vpMath::round(rectangle.getHeight() / m_scale)); } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -2997,13 +2825,11 @@ bool vpDisplayX::getClick(bool blocking) if (ret) { /* Recuperation de la coordonnee du pixel clique. */ - if (XQueryPointer(display, window, &rootwin, &childwin, &root_x, - &root_y, &win_x, &win_y, &modifier)) { + if (XQueryPointer(display, window, &rootwin, &childwin, &root_x, &root_y, &win_x, &win_y, &modifier)) { } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; } @@ -3045,15 +2871,13 @@ bool vpDisplayX::getClick(vpImagePoint &ip, bool blocking) if (ret) { // Get mouse position - if (XQueryPointer(display, window, &rootwin, &childwin, &root_x, - &root_y, &win_x, &win_y, &modifier)) { + if (XQueryPointer(display, window, &rootwin, &childwin, &root_x, &root_y, &win_x, &win_y, &modifier)) { ip.set_u((double)event.xbutton.x * m_scale); ip.set_v((double)event.xbutton.y * m_scale); } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; } @@ -3077,9 +2901,7 @@ bool vpDisplayX::getClick(vpImagePoint &ip, bool blocking) 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 vpDisplayX::getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { bool ret = false; @@ -3101,8 +2923,7 @@ bool vpDisplayX::getClick(vpImagePoint &ip, if (ret) { // Get mouse position - if (XQueryPointer(display, window, &rootwin, &childwin, &root_x, - &root_y, &win_x, &win_y, &modifier)) { + if (XQueryPointer(display, window, &rootwin, &childwin, &root_x, &root_y, &win_x, &win_y, &modifier)) { ip.set_u((double)event.xbutton.x * m_scale); ip.set_v((double)event.xbutton.y * m_scale); switch (event.xbutton.button) { @@ -3119,8 +2940,7 @@ bool vpDisplayX::getClick(vpImagePoint &ip, } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; } @@ -3148,9 +2968,7 @@ bool vpDisplayX::getClick(vpImagePoint &ip, \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool) */ -bool vpDisplayX::getClickUp(vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, - bool blocking) +bool vpDisplayX::getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { bool ret = false; @@ -3171,8 +2989,7 @@ bool vpDisplayX::getClickUp(vpImagePoint &ip, if (ret) { /* Recuperation de la coordonnee du pixel clique. */ - if (XQueryPointer(display, window, &rootwin, &childwin, &root_x, - &root_y, &win_x, &win_y, &modifier)) { + if (XQueryPointer(display, window, &rootwin, &childwin, &root_x, &root_y, &win_x, &win_y, &modifier)) { ip.set_u((double)event.xbutton.x * m_scale); ip.set_v((double)event.xbutton.y * m_scale); switch (event.xbutton.button) { @@ -3189,8 +3006,7 @@ bool vpDisplayX::getClickUp(vpImagePoint &ip, } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; } @@ -3207,11 +3023,9 @@ void vpDisplayX::getImage(vpImage &I) if (m_displayHasBeenInitialized) { XImage *xi; - XCopyArea(display, window, pixmap, context, 0, 0, m_width, m_height, 0, - 0); + XCopyArea(display, window, pixmap, context, 0, 0, m_width, m_height, 0, 0); - xi = XGetImage(display, pixmap, 0, 0, m_width, m_height, AllPlanes, - ZPixmap); + xi = XGetImage(display, pixmap, 0, 0, m_width, m_height, AllPlanes, ZPixmap); I.resize(m_height, m_width); @@ -3261,8 +3075,7 @@ void vpDisplayX::getImage(vpImage &I) } XDestroyImage(xi); } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } } @@ -3276,8 +3089,7 @@ unsigned int vpDisplayX::getScreenDepth() unsigned int depth; if ((display_ = XOpenDisplay(NULL)) == NULL) { - throw(vpDisplayException(vpDisplayException::connexionError, - "Can't connect display on server %s.", + throw(vpDisplayException(vpDisplayException::connexionError, "Can't connect display on server %s.", XDisplayName(NULL))); } screen_ = DefaultScreen(display_); @@ -3298,8 +3110,7 @@ void vpDisplayX::getScreenSize(unsigned int &w, unsigned int &h) int screen_; if ((display_ = XOpenDisplay(NULL)) == NULL) { - throw(vpDisplayException(vpDisplayException::connexionError, - "Can't connect display on server %s.", + throw(vpDisplayException(vpDisplayException::connexionError, "Can't connect display on server %s.", XDisplayName(NULL))); } screen_ = DefaultScreen(display_); @@ -3364,8 +3175,7 @@ bool vpDisplayX::getKeyboardEvent(bool blocking) } } else { vpERROR_TRACE("X not initialized "); - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; } @@ -3404,21 +3214,18 @@ bool vpDisplayX::getKeyboardEvent(std::string &key, bool blocking) // Event testing if (blocking) { XMaskEvent(display, KeyPressMask, &event); - /* count = */ XLookupString((XKeyEvent *)&event, &buffer, 1, &keysym, - &compose_status); + /* count = */ XLookupString((XKeyEvent *)&event, &buffer, 1, &keysym, &compose_status); key = buffer; ret = true; } else { ret = XCheckMaskEvent(display, KeyPressMask, &event); if (ret) { - /* count = */ XLookupString((XKeyEvent *)&event, &buffer, 1, &keysym, - &compose_status); + /* count = */ XLookupString((XKeyEvent *)&event, &buffer, 1, &keysym, &compose_status); key = buffer; } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; } @@ -3448,15 +3255,13 @@ bool vpDisplayX::getPointerMotionEvent(vpImagePoint &ip) if (ret) { // Get mouse position - if (XQueryPointer(display, window, &rootwin, &childwin, &root_x, - &root_y, &win_x, &win_y, &modifier)) { + if (XQueryPointer(display, window, &rootwin, &childwin, &root_x, &root_y, &win_x, &win_y, &modifier)) { ip.set_u((double)event.xbutton.x * m_scale); ip.set_v((double)event.xbutton.y * m_scale); } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; } @@ -3485,15 +3290,13 @@ bool vpDisplayX::getPointerPosition(vpImagePoint &ip) if (ret) { // Get mouse position - if (XQueryPointer(display, window, &rootwin, &childwin, &root_x, - &root_y, &win_x, &win_y, &modifier)) { + if (XQueryPointer(display, window, &rootwin, &childwin, &root_x, &root_y, &win_x, &win_y, &modifier)) { ip.set_u((double)win_x * m_scale); ip.set_v((double)win_y * m_scale); } } } else { - throw(vpDisplayException(vpDisplayException::notInitializedError, - "X not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized")); } return ret; } diff --git a/modules/gui/src/display/windows/vpD3DRenderer.cpp b/modules/gui/src/display/windows/vpD3DRenderer.cpp index c412295412..bcc74a4221 100644 --- a/modules/gui/src/display/windows/vpD3DRenderer.cpp +++ b/modules/gui/src/display/windows/vpD3DRenderer.cpp @@ -83,58 +83,41 @@ vpD3DRenderer::vpD3DRenderer() // D3D palette vpColor pcolor; // Predefined colors pcolor = vpColor::black; - colors[vpColor::id_black] = - D3DCOLOR_ARGB(0xFF, pcolor.R, pcolor.G, pcolor.B); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + colors[vpColor::id_purple] = D3DCOLOR_ARGB(0xFF, pcolor.R, pcolor.G, pcolor.B); // initialize the GDI palette pcolor = vpColor::black; @@ -175,9 +158,8 @@ vpD3DRenderer::vpD3DRenderer() 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); + 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); } /*! @@ -229,13 +211,11 @@ bool vpD3DRenderer::init(HWND hwnd, unsigned int width, unsigned int height) // D3D initialize if (NULL == (pD3D = Direct3DCreate9(D3D_SDK_VERSION))) - throw vpDisplayException(vpDisplayException::notInitializedError, - "Can't initialize D3D!"); + 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!"); + throw vpDisplayException(vpDisplayException::notInitializedError, "Can't get the adapter's display mode!"); D3DPRESENT_PARAMETERS d3dpp; ZeroMemory(&d3dpp, sizeof(d3dpp)); @@ -246,11 +226,8 @@ bool vpD3DRenderer::init(HWND hwnd, unsigned int width, unsigned int height) // 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!"); + 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); @@ -264,25 +241,20 @@ bool vpD3DRenderer::init(HWND hwnd, unsigned int width, unsigned int height) // 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, + if (D3DXCreateTexture(pd3dDevice, textWidth, textWidth, D3DX_DEFAULT, 0, D3DFMT_X8R8G8B8, D3DPOOL_SYSTEMMEM, &pd3dText) != D3D_OK) { - throw vpDisplayException(vpDisplayException::notInitializedError, - "Can't create memory texture!"); + 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!"); + 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!"); + throw vpDisplayException(vpDisplayException::notInitializedError, "Can't create the texture's sprite!"); return true; } @@ -303,8 +275,7 @@ void vpD3DRenderer::initView(float WindowWidth, float WindowHeight) 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!"); + throw vpDisplayException(vpDisplayException::notInitializedError, "Can't set the view!"); } /*! @@ -313,8 +284,7 @@ void vpD3DRenderer::initView(float WindowWidth, float WindowHeight) \param imBuffer Destination buffer. \param pitch Pitch of the destination texture. */ -void vpD3DRenderer::convert(const vpImage &I, unsigned char *imBuffer, - unsigned int pitch) +void vpD3DRenderer::convert(const vpImage &I, unsigned char *imBuffer, unsigned int pitch) { if (m_rscale == 1) { for (unsigned int i = 0; i < m_rheight; i++) { @@ -350,8 +320,7 @@ void vpD3DRenderer::convert(const vpImage &I, unsigned char *imBuffer, \param imBuffer Destination buffer. \param pitch Pitch of the destination texture. */ -void vpD3DRenderer::convert(const vpImage &I, - unsigned char *imBuffer, unsigned int pitch) +void vpD3DRenderer::convert(const vpImage &I, unsigned char *imBuffer, unsigned int pitch) { if (m_rscale == 1) { for (unsigned int i = 0; i < m_rheight; i++) { @@ -389,9 +358,8 @@ Converts a ViSP gray image to the Direct3D texture format (BGRA). \param i_min, i_min Upper/left coordinate of the roi. \param i_max, j_max Bottom/right coordinate of the roi. */ -void vpD3DRenderer::convertROI(const vpImage &I, - unsigned char *imBuffer, unsigned int pitch, - int i_min, int j_min, int i_max, int j_max) +void vpD3DRenderer::convertROI(const vpImage &I, unsigned char *imBuffer, unsigned int pitch, int i_min, + int j_min, int i_max, int j_max) { int h = i_max - i_min; int w = j_max - j_min; @@ -433,9 +401,8 @@ Converts a ViSP gray image to the Direct3D texture format (BGRA). \param i_min, i_min Upper/left coordinate of the roi. \param i_max, j_max Bottom/right coordinate of the roi. */ -void vpD3DRenderer::convertROI(const vpImage &I, - unsigned char *imBuffer, unsigned int pitch, - int i_min, int j_min, int i_max, int j_max) +void vpD3DRenderer::convertROI(const vpImage &I, unsigned char *imBuffer, unsigned int pitch, int i_min, + int j_min, int i_max, int j_max) { int h = i_max - i_min; int w = j_max - j_min; @@ -508,9 +475,7 @@ void vpD3DRenderer::setImg(const vpImage &im) Sets the image to display. \param im The image to display. */ -void vpD3DRenderer::setImgROI(const vpImage &im, - const vpImagePoint &iP, - const unsigned int width, +void vpD3DRenderer::setImgROI(const vpImage &im, const vpImagePoint &iP, const unsigned int width, const unsigned int height) { // if the device has been initialized @@ -519,10 +484,8 @@ void vpD3DRenderer::setImgROI(const vpImage &im, int i_min = (std::max)((int)ceil(iP.get_i() / m_rscale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_rscale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + height) / m_rscale), - (int)m_rheight); - int j_max = - (std::min)((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); + int i_max = (std::min)((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); + int j_max = (std::min)((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); RECT r; r.top = (LONG)i_min; @@ -588,9 +551,7 @@ void vpD3DRenderer::setImg(const vpImage &im) Sets the image to display. \param im The image to display. */ -void vpD3DRenderer::setImgROI(const vpImage &im, - const vpImagePoint &iP, - const unsigned int width, +void vpD3DRenderer::setImgROI(const vpImage &im, const vpImagePoint &iP, const unsigned int width, const unsigned int height) { // if the device has been initialized @@ -599,10 +560,8 @@ void vpD3DRenderer::setImgROI(const vpImage &im, int i_min = (std::max)((int)ceil(iP.get_i() / m_rscale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_rscale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + height) / m_rscale), - (int)m_rheight); - int j_max = - (std::min)((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); + int i_max = (std::min)((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); + int j_max = (std::min)((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); RECT r; r.top = (LONG)i_min; @@ -682,8 +641,7 @@ bool vpD3DRenderer::render() void vpD3DRenderer::setPixel(const vpImagePoint &iP, const vpColor &color) { vpImagePoint iPscaled = iP / m_rscale; - if (iPscaled.get_i() < 0 || iPscaled.get_j() < 0 || - iPscaled.get_i() >= (int)m_rheight || + if (iPscaled.get_i() < 0 || iPscaled.get_j() < 0 || iPscaled.get_i() >= (int)m_rheight || iPscaled.get_j() >= (int)m_rwidth) { return; } @@ -725,9 +683,8 @@ void vpD3DRenderer::setPixel(const vpImagePoint &iP, const vpColor &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) +void vpD3DRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, + unsigned int thickness, int style) { // if the device has been initialized if (pd3dDevice != NULL) { @@ -744,8 +701,7 @@ void vpD3DRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, // create the pen HPEN hPen; if (color.id < vpColor::id_unknown) - hPen = - CreatePen(style, static_cast(thickness), colorsGDI[color.id]); + hPen = CreatePen(style, static_cast(thickness), colorsGDI[color.id]); else { COLORREF gdicolor = RGB(color.R, color.G, color.B); hPen = CreatePen(style, static_cast(thickness), gdicolor); @@ -762,30 +718,23 @@ void vpD3DRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, // line manually drawing multiple small lines if (thickness != 1 && style != PS_SOLID) { double size = 10. * m_rscale; - double length = sqrt(vpMath::sqr(ip2.get_i() - ip1.get_i()) + - vpMath::sqr(ip2.get_j() - ip1.get_j())); + double length = sqrt(vpMath::sqr(ip2.get_i() - ip1.get_i()) + vpMath::sqr(ip2.get_j() - ip1.get_j())); double deltaj = size / length * (ip2.get_j() - ip1.get_j()); double deltai = size / length * (ip2.get_i() - ip1.get_i()); - double slope = - (ip2.get_i() - ip1.get_i()) / (ip2.get_j() - ip1.get_j()); + double slope = (ip2.get_i() - ip1.get_i()) / (ip2.get_j() - ip1.get_j()); double orig = ip1.get_i() - slope * ip1.get_j(); - for (unsigned int j = (unsigned int)ip1.get_j(); j < ip2.get_j(); - j += (unsigned int)(2 * deltaj)) { + for (unsigned int j = (unsigned int)ip1.get_j(); j < ip2.get_j(); j += (unsigned int)(2 * deltaj)) { double i = slope * j + orig; // move to the starting point - MoveToEx(hDCMem, vpMath::round(j / m_rscale), - vpMath::round(i / m_rscale), NULL); + MoveToEx(hDCMem, vpMath::round(j / m_rscale), vpMath::round(i / m_rscale), NULL); // Draw the line - LineTo(hDCMem, vpMath::round((j + deltaj) / m_rscale), - vpMath::round((i + deltai) / m_rscale)); + LineTo(hDCMem, vpMath::round((j + deltaj) / m_rscale), vpMath::round((i + deltai) / m_rscale)); } } else { // move to the starting point - MoveToEx(hDCMem, vpMath::round(ip1.get_u() / m_rscale), - vpMath::round(ip1.get_v() / m_rscale), NULL); + MoveToEx(hDCMem, vpMath::round(ip1.get_u() / m_rscale), vpMath::round(ip1.get_v() / m_rscale), NULL); // Draw the line - LineTo(hDCMem, vpMath::round(ip2.get_u() / m_rscale), - vpMath::round(ip2.get_v() / m_rscale)); + LineTo(hDCMem, vpMath::round(ip2.get_u() / m_rscale), vpMath::round(ip2.get_v() / m_rscale)); } // Releases the DC @@ -806,28 +755,23 @@ void vpD3DRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, \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, +void vpD3DRenderer::drawRect(const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill, unsigned int thickness) { // if the device has been initialized if (pd3dDevice != NULL) { if (fill == false) { drawLine(topLeft, topLeft + vpImagePoint(0, width), color, thickness); - drawLine(topLeft + vpImagePoint(0, width), - topLeft + vpImagePoint(height, width), color, thickness); - drawLine(topLeft + vpImagePoint(height, width), - topLeft + vpImagePoint(height, 0), color, thickness); + drawLine(topLeft + vpImagePoint(0, width), topLeft + vpImagePoint(height, width), color, thickness); + drawLine(topLeft + vpImagePoint(height, width), topLeft + vpImagePoint(height, 0), color, thickness); drawLine(topLeft + vpImagePoint(height, 0), topLeft, color, thickness); } else { vpImagePoint topLeftScaled = topLeft / m_rscale; unsigned int widthScaled = width / m_rscale; unsigned int heightScaled = height / m_rscale; - if (topLeftScaled.get_i() > (int)m_rheight - 1 || - topLeftScaled.get_j() > (int)m_rwidth - 1 || - topLeftScaled.get_i() + height < 0 || - topLeftScaled.get_j() + width < 0) { + if (topLeftScaled.get_i() > (int)m_rheight - 1 || topLeftScaled.get_j() > (int)m_rwidth - 1 || + topLeftScaled.get_i() + height < 0 || topLeftScaled.get_j() + width < 0) { // vpCERROR<<"Invalid parameters!"< 0) ? topLeftScaled.get_i() : 0); - r.left = - (LONG)((topLeftScaled.get_j() > 0) ? topLeftScaled.get_j() : 0); - r.bottom = - (LONG)((topLeftScaled.get_i() + heightScaled < (int)m_rheight) - ? topLeftScaled.get_i() + heightScaled - : m_rheight - 1); - r.right = (LONG)((topLeftScaled.get_j() + widthScaled < (int)m_rwidth) - ? topLeftScaled.get_j() + widthScaled - : m_rwidth - 1); + r.left = (LONG)((topLeftScaled.get_j() > 0) ? topLeftScaled.get_j() : 0); + r.bottom = (LONG)((topLeftScaled.get_i() + heightScaled < (int)m_rheight) ? topLeftScaled.get_i() + heightScaled + : m_rheight - 1); + r.right = (LONG)((topLeftScaled.get_j() + widthScaled < (int)m_rwidth) ? topLeftScaled.get_j() + widthScaled + : m_rwidth - 1); /* unsigned */ int rectW = r.right - r.left; /* unsigned */ int rectH = r.bottom - r.top; @@ -859,10 +799,8 @@ void vpD3DRenderer::drawRect(const vpImagePoint &topLeft, unsigned int width, unsigned int pitch = static_cast(d3dLRect.Pitch); unsigned char *buf = (unsigned char *)d3dLRect.pBits; - if (topLeftScaled.get_i() >= 0 && - topLeftScaled.get_j() + widthScaled < m_rwidth && - topLeftScaled.get_i() + heightScaled < m_rheight && - topLeftScaled.get_j() >= 0) { + if (topLeftScaled.get_i() >= 0 && topLeftScaled.get_j() + widthScaled < m_rwidth && + topLeftScaled.get_i() + heightScaled < m_rheight && topLeftScaled.get_j() >= 0) { for (int x = 0; x < rectW; x++) { for (int y = 0; y < rectH; y++) setBufferPixel(buf, pitch, x, y, color); @@ -922,8 +860,7 @@ void vpD3DRenderer::clear(const vpColor &color) // 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, +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) { @@ -954,14 +891,12 @@ void vpD3DRenderer::subDrawCircle(int i, int j, int x, int y, vpColor col, \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*/) +void vpD3DRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool /*fill*/, + unsigned int /*thickness*/) { unsigned int radiusScaled = radius / m_rscale; vpImagePoint centerScaled = center / m_rscale; - if (radiusScaled < 1 || - vpMath::round(centerScaled.get_i() + radiusScaled) < 0 || + if (radiusScaled < 1 || vpMath::round(centerScaled.get_i() + radiusScaled) < 0 || vpMath::round(centerScaled.get_i() - radiusScaled) > (int)m_rheight || vpMath::round(centerScaled.get_j() + radiusScaled) < 0 || vpMath::round(centerScaled.get_j() - radiusScaled) > (int)m_rwidth) @@ -982,14 +917,12 @@ void vpD3DRenderer::drawCircle(const vpImagePoint ¢er, rec.top = rtop; rec.left = rleft; - rec.bottom = (LONG)( - (vpMath::round(centerScaled.get_i() + radiusScaled_) < (int)m_rheight) - ? centerScaled.get_i() + radiusScaled_ - : m_rheight - 1); - rec.right = (LONG)( - (vpMath::round(centerScaled.get_j() + radiusScaled_) < (int)m_rwidth) - ? centerScaled.get_j() + radiusScaled_ - : m_rwidth - 1); + rec.bottom = (LONG)((vpMath::round(centerScaled.get_i() + radiusScaled_) < (int)m_rheight) + ? centerScaled.get_i() + radiusScaled_ + : m_rheight - 1); + rec.right = (LONG)((vpMath::round(centerScaled.get_j() + radiusScaled_) < (int)m_rwidth) + ? centerScaled.get_j() + radiusScaled_ + : m_rwidth - 1); // used as maxX and maxY for setBufferPixel unsigned int rectW = static_cast(rec.right - rleft); @@ -1015,8 +948,7 @@ void vpD3DRenderer::drawCircle(const vpImagePoint ¢er, ip.set_i(centerScaled.get_i() - rtop); ip.set_j(centerScaled.get_j() - rleft); - subDrawCircle(vpMath::round(ip.get_i()), vpMath::round(ip.get_j()), x, y, - color, buf, pitch, rectW, rectH); + 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) { @@ -1025,8 +957,7 @@ void vpD3DRenderer::drawCircle(const vpImagePoint ¢er, 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); + subDrawCircle(vpMath::round(ip.get_i()), vpMath::round(ip.get_j()), x, y, color, buf, pitch, rectW, rectH); } // unlocks the texture @@ -1041,8 +972,7 @@ void vpD3DRenderer::drawCircle(const vpImagePoint ¢er, \param text The string to display \param color The text's color */ -void vpD3DRenderer::drawText(const vpImagePoint &ip, const char *text, - const vpColor &color) +void vpD3DRenderer::drawText(const vpImagePoint &ip, const char *text, const vpColor &color) { // Will contain the texture's surface drawing context HDC hDCMem; @@ -1075,8 +1005,7 @@ void vpD3DRenderer::drawText(const vpImagePoint &ip, const char *text, GetTextExtentPoint32(hDCMem, text, length, &size); // displays the string - TextOut(hDCMem, vpMath::round(ip.get_u() / m_rscale), - vpMath::round(ip.get_v() / m_rscale), text, length); + TextOut(hDCMem, vpMath::round(ip.get_u() / m_rscale), vpMath::round(ip.get_v() / m_rscale), text, length); // Releases the DC pd3dSurf->ReleaseDC(hDCMem); @@ -1093,8 +1022,7 @@ void vpD3DRenderer::drawText(const vpImagePoint &ip, const char *text, \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) +void vpD3DRenderer::drawCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness) { double i = ip.get_i(); double j = ip.get_j(); @@ -1121,10 +1049,8 @@ void vpD3DRenderer::drawCross(const vpImagePoint &ip, unsigned int size, \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) +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(); @@ -1165,8 +1091,7 @@ void vpD3DRenderer::drawArrow(const vpImagePoint &ip1, \param imBuffer The texture's data. \param pitch The texture's pitch. */ -void TextureToRGBa(vpImage &I, unsigned char *imBuffer, - unsigned int pitch) +void TextureToRGBa(vpImage &I, unsigned char *imBuffer, unsigned int pitch) { unsigned int j = I.getWidth(); diff --git a/modules/gui/src/display/windows/vpDisplayD3D.cpp b/modules/gui/src/display/windows/vpDisplayD3D.cpp index f2db82d0d3..13d4fbac2c 100644 --- a/modules/gui/src/display/windows/vpDisplayD3D.cpp +++ b/modules/gui/src/display/windows/vpDisplayD3D.cpp @@ -60,8 +60,7 @@ vpDisplayD3D::vpDisplayD3D() : vpDisplayWin32(new vpD3DRenderer()) {} index). \param title Window's title. */ -vpDisplayD3D::vpDisplayD3D(int winx, int winy, const std::string &title) - : vpDisplayWin32(new vpD3DRenderer()) +vpDisplayD3D::vpDisplayD3D(int winx, int winy, const std::string &title) : vpDisplayWin32(new vpD3DRenderer()) { m_windowXPosition = winx; m_windowYPosition = winy; @@ -95,8 +94,7 @@ and the columns. and the columns. */ -vpDisplayD3D::vpDisplayD3D(vpImage &I, vpScaleType scaleType) - : vpDisplayWin32(new vpD3DRenderer()) +vpDisplayD3D::vpDisplayD3D(vpImage &I, vpScaleType scaleType) : vpDisplayWin32(new vpD3DRenderer()) { setScale(scaleType, I.getWidth(), I.getHeight()); init(I); @@ -125,8 +123,7 @@ and the columns. and the columns. */ -vpDisplayD3D::vpDisplayD3D(vpImage &I, int winx, int winy, - const std::string &title, vpScaleType scaleType) +vpDisplayD3D::vpDisplayD3D(vpImage &I, int winx, int winy, const std::string &title, vpScaleType scaleType) : vpDisplayWin32(new vpD3DRenderer()) { setScale(scaleType, I.getWidth(), I.getHeight()); @@ -154,8 +151,7 @@ and the columns. and the columns. */ -vpDisplayD3D::vpDisplayD3D(vpImage &I, vpScaleType scaleType) - : vpDisplayWin32(new vpD3DRenderer()) +vpDisplayD3D::vpDisplayD3D(vpImage &I, vpScaleType scaleType) : vpDisplayWin32(new vpD3DRenderer()) { setScale(scaleType, I.getWidth(), I.getHeight()); init(I); @@ -184,8 +180,8 @@ and the columns. and the columns. */ -vpDisplayD3D::vpDisplayD3D(vpImage &I, int winx, int winy, - const std::string &title, vpScaleType scaleType) +vpDisplayD3D::vpDisplayD3D(vpImage &I, int winx, int winy, const std::string &title, + vpScaleType scaleType) : vpDisplayWin32(new vpD3DRenderer()) { setScale(scaleType, I.getWidth(), I.getHeight()); diff --git a/modules/gui/src/display/windows/vpDisplayGDI.cpp b/modules/gui/src/display/windows/vpDisplayGDI.cpp index 273a4ff01d..b4b9ef051a 100644 --- a/modules/gui/src/display/windows/vpDisplayGDI.cpp +++ b/modules/gui/src/display/windows/vpDisplayGDI.cpp @@ -62,8 +62,7 @@ vpDisplayGDI::vpDisplayGDI() : vpDisplayWin32(new vpGDIRenderer()) {} index). \param title Window's title. */ -vpDisplayGDI::vpDisplayGDI(int winx, int winy, const std::string &title) - : vpDisplayWin32(new vpGDIRenderer()) +vpDisplayGDI::vpDisplayGDI(int winx, int winy, const std::string &title) : vpDisplayWin32(new vpGDIRenderer()) { m_windowXPosition = winx; m_windowYPosition = winy; @@ -95,8 +94,7 @@ vpDisplayGDI::vpDisplayGDI(int winx, int winy, const std::string &title) and the columns. */ -vpDisplayGDI::vpDisplayGDI(vpImage &I, vpScaleType scaleType) - : vpDisplayWin32(new vpGDIRenderer()) +vpDisplayGDI::vpDisplayGDI(vpImage &I, vpScaleType scaleType) : vpDisplayWin32(new vpGDIRenderer()) { setScale(scaleType, I.getWidth(), I.getHeight()); init(I); @@ -125,8 +123,7 @@ vpDisplayGDI::vpDisplayGDI(vpImage &I, vpScaleType scaleType) and the columns. */ -vpDisplayGDI::vpDisplayGDI(vpImage &I, int winx, int winy, - const std::string &title, vpScaleType scaleType) +vpDisplayGDI::vpDisplayGDI(vpImage &I, int winx, int winy, const std::string &title, vpScaleType scaleType) : vpDisplayWin32(new vpGDIRenderer()) { setScale(scaleType, I.getWidth(), I.getHeight()); @@ -154,8 +151,7 @@ vpDisplayGDI::vpDisplayGDI(vpImage &I, int winx, int winy, and the columns. */ -vpDisplayGDI::vpDisplayGDI(vpImage &I, vpScaleType scaleType) - : vpDisplayWin32(new vpGDIRenderer()) +vpDisplayGDI::vpDisplayGDI(vpImage &I, vpScaleType scaleType) : vpDisplayWin32(new vpGDIRenderer()) { setScale(scaleType, I.getWidth(), I.getHeight()); init(I); @@ -184,8 +180,8 @@ vpDisplayGDI::vpDisplayGDI(vpImage &I, vpScaleType scaleType) and the columns. */ -vpDisplayGDI::vpDisplayGDI(vpImage &I, int winx, int winy, - const std::string &title, vpScaleType scaleType) +vpDisplayGDI::vpDisplayGDI(vpImage &I, int winx, int winy, const std::string &title, + vpScaleType scaleType) : vpDisplayWin32(new vpGDIRenderer()) { setScale(scaleType, I.getWidth(), I.getHeight()); diff --git a/modules/gui/src/display/windows/vpDisplayWin32.cpp b/modules/gui/src/display/windows/vpDisplayWin32.cpp index 3862cce8a4..61c41c6e2e 100644 --- a/modules/gui/src/display/windows/vpDisplayWin32.cpp +++ b/modules/gui/src/display/windows/vpDisplayWin32.cpp @@ -52,19 +52,14 @@ const int vpDisplayWin32::MAX_INIT_DELAY = 5000; void vpCreateWindow(threadParam *param) { // char* title = param->title; - (param->vpDisp) - ->window.initWindow(param->title.c_str(), param->x, param->y, param->w, - param->h); + (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) -{ -} +vpDisplayWin32::vpDisplayWin32(vpWin32Renderer *rend) : iStatus(false), window(rend) {} /*! Destructor. @@ -81,13 +76,11 @@ vpDisplayWin32::~vpDisplayWin32() { closeDisplay(); } \param title : Window title. */ -void vpDisplayWin32::init(vpImage &I, int x, int y, - const std::string &title) +void vpDisplayWin32::init(vpImage &I, int x, int y, const std::string &title) { if ((I.getHeight() == 0) || (I.getWidth() == 0)) { vpERROR_TRACE("Image not initialized "); - throw(vpDisplayException(vpDisplayException::notInitializedError, - "Image not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized")); } setScale(m_scaleType, I.getWidth(), I.getHeight()); @@ -107,13 +100,11 @@ void vpDisplayWin32::init(vpImage &I, int x, int y, \param x, y : The window is set at position x,y (column index, row index). \param title : Window title. */ -void vpDisplayWin32::init(vpImage &I, int x, int y, - const std::string &title) +void vpDisplayWin32::init(vpImage &I, int x, int y, const std::string &title) { if ((I.getHeight() == 0) || (I.getWidth() == 0)) { vpERROR_TRACE("Image not initialized "); - throw(vpDisplayException(vpDisplayException::notInitializedError, - "Image not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized")); } setScale(m_scaleType, I.getWidth(), I.getHeight()); @@ -133,8 +124,7 @@ void vpDisplayWin32::init(vpImage &I, int x, int y, \param title : Window title. */ -void vpDisplayWin32::init(unsigned int width, unsigned int height, int x, - int y, const std::string &title) +void vpDisplayWin32::init(unsigned int width, unsigned int height, int x, int y, const std::string &title) { if (!title.empty()) m_title = title; @@ -157,8 +147,7 @@ void vpDisplayWin32::init(unsigned int width, unsigned int height, int x, param->title = this->m_title; // creates the window in a separate thread - hThread = CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)vpCreateWindow, - param, 0, &threadId); + hThread = CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)vpCreateWindow, param, 0, &threadId); // the initialization worked iStatus = (hThread != (HANDLE)NULL); @@ -176,8 +165,7 @@ void vpDisplayWin32::waitForInit() if (!window.isInitialized()) { // wait if (WAIT_OBJECT_0 != WaitForSingleObject(window.semaInit, MAX_INIT_DELAY)) - throw(vpDisplayException(vpDisplayException::notInitializedError, - "Window not initialized")); + throw(vpDisplayException(vpDisplayException::notInitializedError, "Window not initialized")); // problem : the window is not initialized } } @@ -221,9 +209,7 @@ void vpDisplayWin32::displayImage(const vpImage &I) \sa init(), closeDisplay() */ -void vpDisplayWin32::displayImageROI(const vpImage &I, - const vpImagePoint &iP, - const unsigned int width, +void vpDisplayWin32::displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height) { // waits if the window is not initialized @@ -274,9 +260,7 @@ void vpDisplayWin32::displayImage(const vpImage &I) \sa init(), closeDisplay() */ -void vpDisplayWin32::displayImageROI(const vpImage &I, - const vpImagePoint &iP, - const unsigned int width, +void vpDisplayWin32::displayImageROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height) { // waits if the window is not initialized @@ -385,9 +369,7 @@ bool vpDisplayWin32::getClick(vpImagePoint &ip, bool blocking) button is pressed, the location of the mouse pointer is updated in \e ip. */ -bool vpDisplayWin32::getClick(vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, - bool blocking) +bool vpDisplayWin32::getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { // wait if the window is not initialized waitForInit(); @@ -435,9 +417,7 @@ bool vpDisplayWin32::getClick(vpImagePoint &ip, \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool) */ -bool vpDisplayWin32::getClickUp(vpImagePoint &ip, - vpMouseButton::vpMouseButtonType &button, - bool blocking) +bool vpDisplayWin32::getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking) { // wait if the window is not initialized waitForInit(); @@ -609,8 +589,7 @@ void vpDisplayWin32::setWindowPosition(int winx, int winy) // cahange the window position only SetWindowPos(window.hWnd, HWND_TOP, winx, winy, 0, 0, - SWP_ASYNCWINDOWPOS | SWP_NOACTIVATE | SWP_NOZORDER | - SWP_NOSIZE); + SWP_ASYNCWINDOWPOS | SWP_NOACTIVATE | SWP_NOZORDER | SWP_NOSIZE); } /*! @@ -630,10 +609,7 @@ void vpDisplayWin32::setTitle(const std::string &windowtitle) \param fontname : Name of the font. */ -void vpDisplayWin32::setFont(const std::string & /* fontname */) -{ - vpERROR_TRACE("Not yet implemented"); -} +void vpDisplayWin32::setFont(const std::string & /* fontname */) { vpERROR_TRACE("Not yet implemented"); } /*! \brief flush the Win32 buffer @@ -654,9 +630,7 @@ void vpDisplayWin32::flushDisplay() 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) +void vpDisplayWin32::flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height) { // waits if the window is not initialized waitForInit(); @@ -684,17 +658,14 @@ void vpDisplayWin32::flushDisplayROI(const vpImagePoint &iP, \param color : Point color. \param thickness : Point thickness. */ -void vpDisplayWin32::displayPoint(const vpImagePoint &ip, - const vpColor &color, - unsigned int thickness) +void vpDisplayWin32::displayPoint(const vpImagePoint &ip, const vpColor &color, unsigned int thickness) { // wait if the window is not initialized waitForInit(); if (thickness == 1) { window.renderer->setPixel(ip, color); } else { - window.renderer->drawRect(ip, thickness * m_scale, thickness * m_scale, - color, true, 1); + window.renderer->drawRect(ip, thickness * m_scale, thickness * m_scale, color, true, 1); } } @@ -704,9 +675,8 @@ void vpDisplayWin32::displayPoint(const vpImagePoint &ip, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplayWin32::displayLine(const vpImagePoint &ip1, - const vpImagePoint &ip2, - const vpColor &color, unsigned int thickness) +void vpDisplayWin32::displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, + unsigned int thickness) { // wait if the window is not initialized waitForInit(); @@ -722,9 +692,7 @@ void vpDisplayWin32::displayLine(const vpImagePoint &ip1, \param color : Line color. \param thickness : Line thickness. */ -void vpDisplayWin32::displayDotLine(const vpImagePoint &ip1, - const vpImagePoint &ip2, - const vpColor &color, +void vpDisplayWin32::displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness) { // wait if the window is not initialized @@ -745,10 +713,8 @@ void vpDisplayWin32::displayDotLine(const vpImagePoint &ip1, \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) +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(); @@ -767,17 +733,13 @@ void vpDisplayWin32::displayRectangle(const vpImagePoint &topLeft, \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) +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(bottomRight.get_j() - topLeft.get_j()); - unsigned int height = - static_cast(bottomRight.get_i() - topLeft.get_i()); + unsigned int width = static_cast(bottomRight.get_j() - topLeft.get_j()); + unsigned int height = static_cast(bottomRight.get_i() - topLeft.get_i()); window.renderer->drawRect(topLeft, width, height, color, fill, thickness); } @@ -792,19 +754,15 @@ void vpDisplayWin32::displayRectangle(const vpImagePoint &topLeft, \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) +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(rectangle.getWidth()), - static_cast(rectangle.getHeight()), - color, fill, thickness); + window.renderer->drawRect(topLeft, static_cast(rectangle.getWidth()), + static_cast(rectangle.getHeight()), color, fill, thickness); } /*! @@ -816,9 +774,8 @@ void vpDisplayWin32::displayRectangle(const vpRect &rectangle, \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) +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(); @@ -831,8 +788,7 @@ void vpDisplayWin32::displayCircle(const vpImagePoint ¢er, \param text : The string to display \param color : The text's color */ -void vpDisplayWin32::displayCharString(const vpImagePoint &ip, - const char *text, const vpColor &color) +void vpDisplayWin32::displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color) { // wait if the window is not initialized waitForInit(); @@ -846,8 +802,7 @@ void vpDisplayWin32::displayCharString(const vpImagePoint &ip, \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, +void vpDisplayWin32::displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness) { // wait if the window is not initialized @@ -862,10 +817,8 @@ void vpDisplayWin32::displayCross(const vpImagePoint &ip, unsigned int size, \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) +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 diff --git a/modules/gui/src/display/windows/vpGDIRenderer.cpp b/modules/gui/src/display/windows/vpGDIRenderer.cpp index 4ec9461c36..9d9f8c4bed 100644 --- a/modules/gui/src/display/windows/vpGDIRenderer.cpp +++ b/modules/gui/src/display/windows/vpGDIRenderer.cpp @@ -47,16 +47,13 @@ /*! Constructor. */ -vpGDIRenderer::vpGDIRenderer() - : m_bmp(NULL), m_bmp_width(0), m_bmp_height(0), timelost(0) +vpGDIRenderer::vpGDIRenderer() : m_bmp(NULL), m_bmp_width(0), m_bmp_height(0), timelost(0) { // 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); + throw vpDisplayException(vpDisplayException::depthNotSupportedError, + "vpGDIRenderer supports only 32bits depth: screen is %dbits depth!", bpp); InitializeCriticalSection(&m_criticalSection); @@ -123,8 +120,7 @@ vpGDIRenderer::~vpGDIRenderer() \param width The window's width. \param height The window's height. */ -bool vpGDIRenderer::init(HWND hWindow, unsigned int width, - unsigned int height) +bool vpGDIRenderer::init(HWND hWindow, unsigned int width, unsigned int height) { timelost = 0.; m_hWnd = hWindow; @@ -133,10 +129,8 @@ bool vpGDIRenderer::init(HWND hWindow, unsigned int width, m_rheight = height; // creates the font - m_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); + m_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; } @@ -156,9 +150,7 @@ void vpGDIRenderer::setImg(const vpImage &I) \param iP : Top left coordinates of the ROI. \param width, height : ROI width and height. */ -void vpGDIRenderer::setImgROI(const vpImage &I, - const vpImagePoint &iP, - const unsigned int width, +void vpGDIRenderer::setImgROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height) { // converts the image into a HBITMAP @@ -181,9 +173,7 @@ void vpGDIRenderer::setImg(const vpImage &I) \param iP : Top left coordinates of the ROI. \param width, height : ROI width and height. */ -void vpGDIRenderer::setImgROI(const vpImage &I, - const vpImagePoint &iP, - const unsigned int width, +void vpGDIRenderer::setImgROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height) { // converts the image into a HBITMAP @@ -207,8 +197,7 @@ bool vpGDIRenderer::render() SelectObject(hDCMem, m_bmp); // blits it on the window's DC - BitBlt(hDCScreen, 0, 0, static_cast(m_rwidth), - static_cast(m_rheight), hDCMem, 0, 0, SRCCOPY); + BitBlt(hDCScreen, 0, 0, static_cast(m_rwidth), static_cast(m_rheight), hDCMem, 0, 0, SRCCOPY); LeaveCriticalSection(&m_criticalSection); // DeleteDC(hDCMem); @@ -230,8 +219,7 @@ void vpGDIRenderer::convert(const vpImage &I, HBITMAP &hBmp) unsigned char *imBuffer = new unsigned char[m_rwidth * m_rheight * 4]; if (m_rscale == 1) { - for (unsigned int i = 0, k = 0; i < m_rwidth * m_rheight * 4; - i += 4, k++) { + for (unsigned int i = 0, k = 0; i < m_rwidth * m_rheight * 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; @@ -265,17 +253,13 @@ void vpGDIRenderer::convert(const vpImage &I, HBITMAP &hBmp) \param iP : Top left coordinates of the ROI. \param width, height : ROI width and height. */ -void vpGDIRenderer::convertROI(const vpImage &I, - const vpImagePoint &iP, - const unsigned int width, +void vpGDIRenderer::convertROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height) { int i_min = (std::max)((int)ceil(iP.get_i() / m_rscale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_rscale), 0); - int i_max = - (std::min)((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); - int j_max = - (std::min)((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); + int i_max = (std::min)((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); + int j_max = (std::min)((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); int h = i_max - i_min; int w = j_max - j_min; @@ -334,8 +318,7 @@ void vpGDIRenderer::convert(const vpImage &I, HBITMAP &hBmp) unsigned char *imBuffer = new unsigned char[m_rwidth * m_rheight * 4]; if (m_rscale == 1) { - for (unsigned int i = 0, k = 0; i < m_rwidth * m_rheight * 4; - i += 4, k++) { + for (unsigned int i = 0, k = 0; i < m_rwidth * m_rheight * 4; i += 4, k++) { imBuffer[i + 0] = I.bitmap[k]; imBuffer[i + 1] = I.bitmap[k]; imBuffer[i + 2] = I.bitmap[k]; @@ -369,17 +352,13 @@ void vpGDIRenderer::convert(const vpImage &I, HBITMAP &hBmp) \param iP : Top left coordinates of the ROI. \param width, height : ROI width and height. */ -void vpGDIRenderer::convertROI(const vpImage &I, - const vpImagePoint &iP, - const unsigned int width, +void vpGDIRenderer::convertROI(const vpImage &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height) { int i_min = (std::max)((int)ceil(iP.get_i() / m_rscale), 0); int j_min = (std::max)((int)ceil(iP.get_j() / m_rscale), 0); - int i_max = - (std::min)((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); - int j_max = - (std::min)((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); + int i_max = (std::min)((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); + int j_max = (std::min)((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); int h = i_max - i_min; int w = j_max - j_min; @@ -432,8 +411,7 @@ void vpGDIRenderer::convertROI(const vpImage &I, \return the operation succefulness */ -bool vpGDIRenderer::updateBitmap(HBITMAP &hBmp, unsigned char *imBuffer, - unsigned int w, unsigned int h) +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 @@ -449,8 +427,7 @@ bool vpGDIRenderer::updateBitmap(HBITMAP &hBmp, unsigned char *imBuffer, DeleteObject(hBmp); } // create a new BITMAP from this buffer - if ((hBmp = CreateBitmap(static_cast(w), static_cast(h), 1, 32, - (void *)imBuffer)) == NULL) + if ((hBmp = CreateBitmap(static_cast(w), static_cast(h), 1, 32, (void *)imBuffer)) == NULL) return false; m_bmp_width = w; @@ -471,8 +448,7 @@ bool vpGDIRenderer::updateBitmap(HBITMAP &hBmp, unsigned char *imBuffer, \return the operation succefulness */ -bool vpGDIRenderer::updateBitmapROI(unsigned char *imBuffer, int i_min, - int j_min, int w, int h) +bool vpGDIRenderer::updateBitmapROI(unsigned char *imBuffer, int i_min, int j_min, int w, int h) { HBITMAP htmp = CreateBitmap(w, h, 1, 32, (void *)imBuffer); @@ -513,12 +489,10 @@ void vpGDIRenderer::setPixel(const vpImagePoint &iP, const vpColor &color) SelectObject(hDCMem, m_bmp); if (color.id < vpColor::id_unknown) - SetPixel(hDCMem, vpMath::round(iP.get_u() / m_rscale), - vpMath::round(iP.get_v() / m_rscale), m_colors[color.id]); + SetPixel(hDCMem, vpMath::round(iP.get_u() / m_rscale), vpMath::round(iP.get_v() / m_rscale), m_colors[color.id]); else { COLORREF gdicolor = RGB(color.R, color.G, color.B); - SetPixel(hDCMem, vpMath::round(iP.get_u() / m_rscale), - vpMath::round(iP.get_v() / m_rscale), gdicolor); + SetPixel(hDCMem, vpMath::round(iP.get_u() / m_rscale), vpMath::round(iP.get_v() / m_rscale), gdicolor); } // display the result (flush) // BitBlt(hDCScreen, x, y, 1, 1, hDCMem, x, y, SRCCOPY); @@ -536,9 +510,8 @@ void vpGDIRenderer::setPixel(const vpImagePoint &iP, const vpColor &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) +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; @@ -556,8 +529,7 @@ void vpGDIRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, // create the pen if (color.id < vpColor::id_unknown) - hPen = - CreatePen(style, static_cast(thickness), m_colors[color.id]); + hPen = CreatePen(style, static_cast(thickness), m_colors[color.id]); else { COLORREF gdicolor = RGB(color.R, color.G, color.B); hPen = CreatePen(style, static_cast(thickness), gdicolor); @@ -621,29 +593,23 @@ void vpGDIRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, // line manually drawing multiple small lines if (thickness != 1 && style != PS_SOLID) { double size = 10. * m_rscale; - double length = sqrt(vpMath::sqr(ip2.get_i() - ip1.get_i()) + - vpMath::sqr(ip2.get_j() - ip1.get_j())); + double length = sqrt(vpMath::sqr(ip2.get_i() - ip1.get_i()) + vpMath::sqr(ip2.get_j() - ip1.get_j())); double deltaj = size / length * (ip2.get_j() - ip1.get_j()); double deltai = size / length * (ip2.get_i() - ip1.get_i()); double slope = (ip2.get_i() - ip1.get_i()) / (ip2.get_j() - ip1.get_j()); double orig = ip1.get_i() - slope * ip1.get_j(); - for (unsigned int j = (unsigned int)ip1.get_j(); j < ip2.get_j(); - j += (unsigned int)(2 * deltaj)) { + for (unsigned int j = (unsigned int)ip1.get_j(); j < ip2.get_j(); j += (unsigned int)(2 * deltaj)) { double i = slope * j + orig; // move to the starting point - MoveToEx(hDCMem, vpMath::round(j / m_rscale), - vpMath::round(i / m_rscale), NULL); + MoveToEx(hDCMem, vpMath::round(j / m_rscale), vpMath::round(i / m_rscale), NULL); // Draw the line - LineTo(hDCMem, vpMath::round((j + deltaj) / m_rscale), - vpMath::round((i + deltai) / m_rscale)); + LineTo(hDCMem, vpMath::round((j + deltaj) / m_rscale), vpMath::round((i + deltai) / m_rscale)); } } else { // move to the starting point - MoveToEx(hDCMem, vpMath::round(ip1.get_u() / m_rscale), - vpMath::round(ip1.get_v() / m_rscale), NULL); + MoveToEx(hDCMem, vpMath::round(ip1.get_u() / m_rscale), vpMath::round(ip1.get_v() / m_rscale), NULL); // Draw the line - LineTo(hDCMem, vpMath::round(ip2.get_u() / m_rscale), - vpMath::round(ip2.get_v() / m_rscale)); + LineTo(hDCMem, vpMath::round(ip2.get_u() / m_rscale), vpMath::round(ip2.get_v() / m_rscale)); } LeaveCriticalSection(&m_criticalSection); @@ -662,8 +628,7 @@ void vpGDIRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, \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, +void vpGDIRenderer::drawRect(const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill, unsigned int thickness) { if (thickness == 0) @@ -677,8 +642,7 @@ void vpGDIRenderer::drawRect(const vpImagePoint &topLeft, unsigned int width, COLORREF gdicolor = RGB(0, 0, 0); if (color.id < vpColor::id_unknown) - hPen = - CreatePen(PS_SOLID, static_cast(thickness), m_colors[color.id]); + hPen = CreatePen(PS_SOLID, static_cast(thickness), m_colors[color.id]); else { gdicolor = RGB(color.R, color.G, color.B); hPen = CreatePen(PS_SOLID, static_cast(thickness), gdicolor); @@ -707,10 +671,8 @@ void vpGDIRenderer::drawRect(const vpImagePoint &topLeft, unsigned int width, SelectObject(hDCMem, hPen); // draw the rectangle - Rectangle(hDCMem, vpMath::round(topLeft.get_u() / m_rscale), - vpMath::round(topLeft.get_v() / m_rscale), - vpMath::round((topLeft.get_u() + width) / m_rscale), - vpMath::round((topLeft.get_v() + height) / m_rscale)); + Rectangle(hDCMem, vpMath::round(topLeft.get_u() / m_rscale), vpMath::round(topLeft.get_v() / m_rscale), + vpMath::round((topLeft.get_u() + width) / m_rscale), vpMath::round((topLeft.get_v() + height) / m_rscale)); // display the result (flush) // BitBlt(hDCScreen, j, i, width, height, hDCMem, j, i, SRCCOPY); @@ -743,9 +705,8 @@ void vpGDIRenderer::clear(const vpColor &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) +void vpGDIRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill, + unsigned int thickness) { // get the window's DC @@ -755,8 +716,7 @@ void vpGDIRenderer::drawCircle(const vpImagePoint ¢er, // create the pen HPEN hPen; if (color.id < vpColor::id_unknown) - hPen = - CreatePen(PS_SOLID, static_cast(thickness), m_colors[color.id]); + hPen = CreatePen(PS_SOLID, static_cast(thickness), m_colors[color.id]); else { COLORREF gdicolor = RGB(color.R, color.G, color.B); hPen = CreatePen(PS_SOLID, static_cast(thickness), gdicolor); @@ -814,8 +774,7 @@ void vpGDIRenderer::drawCircle(const vpImagePoint ¢er, \param text The string to display \param color The text's color */ -void vpGDIRenderer::drawText(const vpImagePoint &ip, const char *text, - const vpColor &color) +void vpGDIRenderer::drawText(const vpImagePoint &ip, const char *text, const vpColor &color) { // get the window's DC HDC hDCScreen = GetDC(m_hWnd); @@ -846,8 +805,7 @@ void vpGDIRenderer::drawText(const vpImagePoint &ip, const char *text, GetTextExtentPoint32(hDCMem, text, length, &size); // displays the string - TextOut(hDCMem, vpMath::round(ip.get_u() / m_rscale), - vpMath::round(ip.get_v() / m_rscale), text, length); + TextOut(hDCMem, vpMath::round(ip.get_u() / m_rscale), vpMath::round(ip.get_v() / m_rscale), text, length); // display the result (flush) // BitBlt(hDCScreen, j, i, size.cx, size.cy, hDCMem, j, i, SRCCOPY); @@ -865,8 +823,7 @@ void vpGDIRenderer::drawText(const vpImagePoint &ip, const char *text, \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) +void vpGDIRenderer::drawCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness) { /* unsigned */ int half_size = static_cast(size / 2 / m_rscale); @@ -881,8 +838,7 @@ void vpGDIRenderer::drawCross(const vpImagePoint &ip, unsigned int size, // create the pen HPEN hPen; if (color.id < vpColor::id_unknown) - hPen = CreatePen(PS_SOLID, static_cast(thickness), - m_colors[color.id]); + hPen = CreatePen(PS_SOLID, static_cast(thickness), m_colors[color.id]); else { COLORREF gdicolor = RGB(color.R, color.G, color.B); hPen = CreatePen(PS_SOLID, static_cast(thickness), gdicolor); @@ -896,18 +852,14 @@ void vpGDIRenderer::drawCross(const vpImagePoint &ip, unsigned int size, SelectObject(hDCMem, hPen); // move to the starting point - MoveToEx(hDCMem, vpMath::round(ip.get_u() / m_rscale) - half_size, - vpMath::round(ip.get_v() / m_rscale), NULL); + MoveToEx(hDCMem, vpMath::round(ip.get_u() / m_rscale) - half_size, vpMath::round(ip.get_v() / m_rscale), NULL); // Draw the first line (horizontal) - LineTo(hDCMem, vpMath::round(ip.get_u() / m_rscale) + half_size, - vpMath::round(ip.get_v() / m_rscale)); + LineTo(hDCMem, vpMath::round(ip.get_u() / m_rscale) + half_size, vpMath::round(ip.get_v() / m_rscale)); // move to the starting point - MoveToEx(hDCMem, vpMath::round(ip.get_u() / m_rscale), - vpMath::round(ip.get_v() / m_rscale) - half_size, NULL); + MoveToEx(hDCMem, vpMath::round(ip.get_u() / m_rscale), vpMath::round(ip.get_v() / m_rscale) - half_size, NULL); // Draw the second line (vertical) - LineTo(hDCMem, vpMath::round(ip.get_u() / m_rscale), - vpMath::round(ip.get_v() / m_rscale) + half_size); + LineTo(hDCMem, vpMath::round(ip.get_u() / m_rscale), vpMath::round(ip.get_v() / m_rscale) + half_size); // display the result (flush) // BitBlt(hDCScreen, j-(size/2), i-(size/2), size, size, @@ -930,10 +882,8 @@ void vpGDIRenderer::drawCross(const vpImagePoint &ip, unsigned int size, \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) +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() / m_rscale - ip1.get_i() / m_rscale; double b = ip2.get_j() / m_rscale - ip1.get_j() / m_rscale; @@ -952,8 +902,7 @@ void vpGDIRenderer::drawArrow(const vpImagePoint &ip1, // create the pen HPEN hPen; if (color.id < vpColor::id_unknown) - hPen = - CreatePen(PS_SOLID, static_cast(thickness), m_colors[color.id]); + hPen = CreatePen(PS_SOLID, static_cast(thickness), m_colors[color.id]); else { COLORREF gdicolor = RGB(color.R, color.G, color.B); hPen = CreatePen(PS_SOLID, static_cast(thickness), gdicolor); @@ -985,10 +934,8 @@ void vpGDIRenderer::drawArrow(const vpImagePoint &ip1, ip4.set_j(ip3.get_j() + a * h); if (lg > 2 * vpImagePoint::distance(ip2 / m_rscale, ip4)) { - MoveToEx(hDCMem, vpMath::round(ip2.get_u() / m_rscale), - vpMath::round(ip2.get_v() / m_rscale), NULL); - LineTo(hDCMem, vpMath::round(ip4.get_u()), - vpMath::round(ip4.get_v())); + MoveToEx(hDCMem, vpMath::round(ip2.get_u() / m_rscale), vpMath::round(ip2.get_v() / m_rscale), NULL); + LineTo(hDCMem, vpMath::round(ip4.get_u()), vpMath::round(ip4.get_v())); } // t+=0.1 ; } @@ -1000,18 +947,14 @@ void vpGDIRenderer::drawArrow(const vpImagePoint &ip1, ip4.set_j(ip3.get_j() - a * h); if (lg > 2 * vpImagePoint::distance(ip2 / m_rscale, ip4)) { - MoveToEx(hDCMem, vpMath::round(ip2.get_u() / m_rscale), - vpMath::round(ip2.get_v() / m_rscale), NULL); - LineTo(hDCMem, vpMath::round(ip4.get_u()), - vpMath::round(ip4.get_v())); + MoveToEx(hDCMem, vpMath::round(ip2.get_u() / m_rscale), vpMath::round(ip2.get_v() / m_rscale), NULL); + LineTo(hDCMem, vpMath::round(ip4.get_u()), vpMath::round(ip4.get_v())); } // t-=0.1 ; } - MoveToEx(hDCMem, vpMath::round(ip1.get_u() / m_rscale), - vpMath::round(ip1.get_v() / m_rscale), NULL); - LineTo(hDCMem, vpMath::round(ip2.get_u() / m_rscale), - vpMath::round(ip2.get_v() / m_rscale)); + MoveToEx(hDCMem, vpMath::round(ip1.get_u() / m_rscale), vpMath::round(ip1.get_v() / m_rscale), NULL); + LineTo(hDCMem, vpMath::round(ip2.get_u() / m_rscale), vpMath::round(ip2.get_v() / m_rscale)); } // display the result (flush) diff --git a/modules/gui/src/display/windows/vpWin32API.cpp b/modules/gui/src/display/windows/vpWin32API.cpp index 8a4bb2e483..da0624bc68 100644 --- a/modules/gui/src/display/windows/vpWin32API.cpp +++ b/modules/gui/src/display/windows/vpWin32API.cpp @@ -47,13 +47,10 @@ 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; + 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; } @@ -73,11 +70,10 @@ BOOL vpMoveToEx(HDC hdc, int X, int Y, LPPOINT lpPoint) return ret; } -BOOL vpBitBlt(HDC hdcDest, int nXDest, int nYDest, int nWidth, int nHeight, - HDC hdcSrc, int nXSrc, int nYSrc, DWORD dwRop) +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); + BOOL ret = BitBlt(hdcDest, nXDest, nYDest, nWidth, nHeight, hdcSrc, nXSrc, nYSrc, dwRop); if (ret == 0) vpProcessErrors("BitBlt"); return ret; @@ -108,8 +104,7 @@ void vpSelectObject(HWND hWnd, HDC hDC, HDC hDCMem, HGDIOBJ h) } } -BOOL vpReleaseSemaphore(HANDLE hSemaphore, LONG IReleaseCount, - LPLONG lpPreviousCount) +BOOL vpReleaseSemaphore(HANDLE hSemaphore, LONG IReleaseCount, LPLONG lpPreviousCount) { BOOL ret = ReleaseSemaphore(hSemaphore, IReleaseCount, lpPreviousCount); #ifndef __MINGW32__ @@ -120,14 +115,8 @@ BOOL vpReleaseSemaphore(HANDLE hSemaphore, LONG IReleaseCount, return ret; } -void vpEnterCriticalSection(LPCRITICAL_SECTION lpCriticalSection) -{ - EnterCriticalSection(lpCriticalSection); -} -void vpLeaveCriticalSection(LPCRITICAL_SECTION lpCriticalSection) -{ - LeaveCriticalSection(lpCriticalSection); -} +void vpEnterCriticalSection(LPCRITICAL_SECTION lpCriticalSection) { EnterCriticalSection(lpCriticalSection); } +void vpLeaveCriticalSection(LPCRITICAL_SECTION lpCriticalSection) { LeaveCriticalSection(lpCriticalSection); } COLORREF vpSetPixel(HDC hdc, int X, int Y, COLORREF crColor) { @@ -137,8 +126,7 @@ COLORREF vpSetPixel(HDC hdc, int X, int Y, COLORREF crColor) return ret; } -HBITMAP vpCreateBitmap(int nWidth, int nHeight, UINT cPlanes, - UINT cBitsPerPel, const VOID *lpvBits) +HBITMAP vpCreateBitmap(int nWidth, int nHeight, UINT cPlanes, UINT cBitsPerPel, const VOID *lpvBits) { HBITMAP ret = CreateBitmap(nWidth, nHeight, cPlanes, cBitsPerPel, lpvBits); if (ret == NULL) diff --git a/modules/gui/src/display/windows/vpWin32Window.cpp b/modules/gui/src/display/windows/vpWin32Window.cpp index 0348b6facd..ec8c118afb 100644 --- a/modules/gui/src/display/windows/vpWin32Window.cpp +++ b/modules/gui/src/display/windows/vpWin32Window.cpp @@ -69,8 +69,7 @@ bool vpWin32Window::registered = false; /*! The message callback */ -LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, - LPARAM lParam) +LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam) { // the first time this callback is executed, put the window in initialized // state @@ -235,8 +234,7 @@ vpWin32Window::~vpWin32Window() \param h Initial window's height */ -void vpWin32Window::initWindow(const char *title, int posx, int posy, - unsigned int w, unsigned int h) +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; @@ -272,17 +270,14 @@ void vpWin32Window::initWindow(const char *title, int posx, int posy, // 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); + 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!"); + 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); + 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; diff --git a/modules/gui/src/forward-projection/vpProjectionDisplay.cpp b/modules/gui/src/forward-projection/vpProjectionDisplay.cpp index 3f635cfd48..581301a742 100644 --- a/modules/gui/src/forward-projection/vpProjectionDisplay.cpp +++ b/modules/gui/src/forward-projection/vpProjectionDisplay.cpp @@ -93,25 +93,19 @@ void vpProjectionDisplay::init(const int select) void vpProjectionDisplay::close() {} -void vpProjectionDisplay::display(vpImage &I, - const vpHomogeneousMatrix &cextMo, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const vpColor &color, - const bool &displayTraj, - const unsigned int thickness) +void vpProjectionDisplay::display(vpImage &I, const vpHomogeneousMatrix &cextMo, + const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, + const bool &displayTraj, const unsigned int thickness) { - for (std::list::const_iterator it = listFp.begin(); - it != listFp.end(); ++it) { + for (std::list::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); + vpDisplay::displayCircle(I, (int)traj[i][0], (int)traj[i][1], 2, vpColor::green, true); displayCamera(I, cextMo, cMo, cam, thickness); @@ -119,15 +113,12 @@ void vpProjectionDisplay::display(vpImage &I, { 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]); + vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], traj[n][1], traj[n][0]); } } -void vpProjectionDisplay::displayCamera(vpImage &I, - const vpHomogeneousMatrix &cextMo, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, +void vpProjectionDisplay::displayCamera(vpImage &I, const vpHomogeneousMatrix &cextMo, + const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const unsigned int thickness) { vpHomogeneousMatrix c1Mc; @@ -148,16 +139,13 @@ void vpProjectionDisplay::displayCamera(vpImage &I, 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); + 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); + 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); + vpDisplay::displayArrow(I, ipo, ipx, vpColor::red, 4 + thickness, 2 + thickness, thickness); } #elif !defined(VISP_BUILD_SHARED_LIBS) diff --git a/modules/gui/src/plot/vpPlot.cpp b/modules/gui/src/plot/vpPlot.cpp index 426ab1e715..f6b6c553f0 100644 --- a/modules/gui/src/plot/vpPlot.cpp +++ b/modules/gui/src/plot/vpPlot.cpp @@ -58,9 +58,7 @@ Needs then a call to init(). */ -vpPlot::vpPlot() - : I(), display(NULL), graphNbr(1), graphList(NULL), margei(30), margej(40), - factori(1.f), factorj(1.) +vpPlot::vpPlot() : I(), display(NULL), graphNbr(1), graphList(NULL), margei(30), margej(40), factori(1.f), factorj(1.) { } /*! @@ -80,11 +78,9 @@ vpPlot::vpPlot() \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 std::string &title) - : I(), display(NULL), graphNbr(1), graphList(NULL), margei(30), margej(40), - factori(1.f), factorj(1.) +vpPlot::vpPlot(const unsigned int graph_nbr, const unsigned int height, const unsigned int width, const int x, + const int y, const std::string &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); } @@ -101,9 +97,8 @@ vpPlot::vpPlot(const unsigned int graph_nbr, const unsigned int height, \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 std::string &title) +void vpPlot::init(const unsigned int graph_nbr, const unsigned int height, const unsigned int width, const int x, + const int y, const std::string &title) { I.init(height, width, 255); @@ -156,47 +151,39 @@ vpPlot::~vpPlot() void vpPlot::initNbGraph(unsigned int nbGraph) { if (nbGraph > 4) { - throw vpException(vpException::dimensionError, - "Cannot create more than 4 graphs"); + 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); + 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), + 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), + 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), + 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), + 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), + 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; } @@ -216,10 +203,7 @@ void vpPlot::initNbGraph(unsigned int nbGraph) 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::initGraph(unsigned int graphNum, unsigned int curveNbr) { (graphList + graphNum)->initGraph(curveNbr); } // void // vpPlot::initRange (const int graphNum, @@ -241,11 +225,9 @@ void vpPlot::initGraph(unsigned int graphNum, unsigned int curveNbr) 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) +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); + (graphList + graphNum)->initScale(I, xmin, xmax, 10, ymin, ymax, 10, true, true); } /*! @@ -262,12 +244,10 @@ void vpPlot::initRange(const unsigned int graphNum, double xmin, double xmax, 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) +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); + (graphList + graphNum)->initScale(I, xmin, xmax, 10, ymin, ymax, 10, zmin, zmax, 10, true, true); } /*! @@ -278,8 +258,7 @@ void vpPlot::initRange(const unsigned int graphNum, double xmin, double xmax, 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) +void vpPlot::setColor(const unsigned int graphNum, const unsigned int curveNum, vpColor color) { (graphList + graphNum)->setCurveColor(curveNum, color); } @@ -304,8 +283,7 @@ void vpPlot::displayGrid() 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) +void vpPlot::plot(const unsigned int graphNum, const unsigned int curveNum, const double x, const double y) { (graphList + graphNum)->plot(I, curveNum, x, y); } @@ -321,8 +299,7 @@ void vpPlot::plot(const unsigned int graphNum, const unsigned int curveNum, 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) +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) @@ -341,8 +318,7 @@ void vpPlot::plot(const unsigned int graphNum, const double x, 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) +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) @@ -362,8 +338,7 @@ void vpPlot::plot(const unsigned int graphNum, const double x, 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) +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) @@ -382,8 +357,7 @@ void vpPlot::plot(const unsigned int graphNum, const double x, 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) +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) @@ -403,8 +377,7 @@ void vpPlot::plot(const unsigned int graphNum, const double x, 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) +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) @@ -426,10 +399,8 @@ void vpPlot::plot(const unsigned int graphNum, const double x, 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) +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); } @@ -446,14 +417,11 @@ vpMouseButton::vpMouseButtonType vpPlot::plot(const unsigned int graphNum, 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, +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()) { + 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 @@ -576,8 +544,7 @@ void vpPlot::setUnitZ(const unsigned int graphNum, const std::string &unitz) 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 std::string &legend) +void vpPlot::setLegend(const unsigned int graphNum, const unsigned int curveNum, const std::string &legend) { (graphList + graphNum)->setLegend(curveNum, legend); } @@ -604,9 +571,7 @@ 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) +void vpPlot::setThickness(const unsigned int graphNum, const unsigned int curveNum, const unsigned int thickness) { (graphList + graphNum)->setCurveThickness(curveNum, thickness); } @@ -619,11 +584,9 @@ belonging to a given graphic. 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) +void vpPlot::setGraphThickness(const unsigned int graphNum, const unsigned int thickness) { - for (unsigned int curveNum = 0; curveNum < (graphList + graphNum)->curveNbr; - curveNum++) + for (unsigned int curveNum = 0; curveNum < (graphList + graphNum)->curveNbr; curveNum++) (graphList + graphNum)->setCurveThickness(curveNum, thickness); } @@ -635,8 +598,7 @@ void vpPlot::setGraphThickness(const unsigned int graphNum, 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) +void vpPlot::setGridThickness(const unsigned int graphNum, const unsigned int thickness) { (graphList + graphNum)->setGridThickness(thickness); } @@ -650,8 +612,7 @@ void vpPlot::setGridThickness(const unsigned int graphNum, 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) +void vpPlot::resetPointList(const unsigned int graphNum, const unsigned int curveNum) { (graphList + graphNum)->resetPointList(curveNum); } @@ -682,9 +643,7 @@ void vpPlot::resetPointList(const unsigned int graphNum, 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 std::string &dataFile, - const std::string &title_prefix) +void vpPlot::saveData(const unsigned int graphNum, const std::string &dataFile, const std::string &title_prefix) { std::ofstream fichier; fichier.open(dataFile.c_str()); @@ -693,22 +652,16 @@ void vpPlot::saveData(const unsigned int graphNum, double *p = new double[3]; bool end = false; - std::vector::const_iterator> vec_iter_pointListx( - (graphList + graphNum)->curveNbr); - std::vector::const_iterator> vec_iter_pointListy( - (graphList + graphNum)->curveNbr); - std::vector::const_iterator> vec_iter_pointListz( - (graphList + graphNum)->curveNbr); + std::vector::const_iterator> vec_iter_pointListx((graphList + graphNum)->curveNbr); + std::vector::const_iterator> vec_iter_pointListy((graphList + graphNum)->curveNbr); + std::vector::const_iterator> vec_iter_pointListz((graphList + graphNum)->curveNbr); fichier << title_prefix << (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(); + 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(); @@ -722,12 +675,9 @@ void vpPlot::saveData(const unsigned int graphNum, // !(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())) { + 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]; @@ -750,12 +700,9 @@ void vpPlot::saveData(const unsigned int graphNum, // !(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())) + 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] = diff --git a/modules/gui/src/plot/vpPlotCurve.cpp b/modules/gui/src/plot/vpPlotCurve.cpp index a0170645e0..28c84a79f6 100644 --- a/modules/gui/src/plot/vpPlotCurve.cpp +++ b/modules/gui/src/plot/vpPlotCurve.cpp @@ -48,9 +48,8 @@ #if defined(VISP_HAVE_DISPLAY) vpPlotCurve::vpPlotCurve() - : color(vpColor::red), curveStyle(point), thickness(1), nbPoint(0), - lastPoint(), pointListx(), pointListy(), pointListz(), legend(), xmin(0), - xmax(0), ymin(0), ymax(0) + : color(vpColor::red), curveStyle(point), thickness(1), nbPoint(0), lastPoint(), pointListx(), pointListy(), + pointListz(), legend(), xmin(0), xmax(0), ymin(0), ymax(0) { } @@ -61,9 +60,7 @@ vpPlotCurve::~vpPlotCurve() pointListz.clear(); } -void vpPlotCurve::plotPoint(const vpImage &I, - const vpImagePoint &iP, const double x, - const double y) +void vpPlotCurve::plotPoint(const vpImage &I, const vpImagePoint &iP, const double x, const double y) { nbPoint++; @@ -98,8 +95,7 @@ void vpPlotCurve::plotPoint(const vpImage &I, pointListz.push_back(0.0); } -void vpPlotCurve::plotList(const vpImage &I, const double xorg, - const double yorg, const double zoomx, +void vpPlotCurve::plotList(const vpImage &I, const double xorg, const double yorg, const double zoomx, const double zoomy) { std::list::const_iterator it_ptListx = pointListx.begin(); diff --git a/modules/gui/src/plot/vpPlotGraph.cpp b/modules/gui/src/plot/vpPlotGraph.cpp index 3527fbf59d..7d15936630 100644 --- a/modules/gui/src/plot/vpPlotGraph.cpp +++ b/modules/gui/src/plot/vpPlotGraph.cpp @@ -59,21 +59,16 @@ #if defined(VISP_HAVE_DISPLAY) int laFonctionSansNom(const double delta); -void getGrid3DPoint(const double pente, vpImagePoint &iPunit, - vpImagePoint &ip1, vpImagePoint &ip2, vpImagePoint &ip3); +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) + : 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); @@ -96,8 +91,7 @@ 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}; + 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]; @@ -108,8 +102,7 @@ void vpPlotGraph::initGraph(unsigned int nbCurve) } } -void vpPlotGraph::initSize(vpImagePoint top_left, unsigned int w, - unsigned int h, unsigned int margei, +void vpPlotGraph::initSize(vpImagePoint top_left, unsigned int w, unsigned int h, unsigned int margei, unsigned int margej) { this->topLeft = top_left; @@ -119,16 +112,14 @@ void vpPlotGraph::initSize(vpImagePoint top_left, unsigned int w, graphZone.setWidth(width); graphZone.setHeight(height); - this->dTopLeft = - vpImagePoint(topLeft.get_i() + margei, topLeft.get_j() + margej); + 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); + this->dTopLeft3D = vpImagePoint(topLeft.get_i() + margei, topLeft.get_j() + 10); dGraphZone3D.setTopLeft(dTopLeft3D); dGraphZone3D.setWidth(dWidth + 1); dGraphZone3D.setHeight(dHeight + 1); @@ -159,8 +150,7 @@ void vpPlotGraph::initSize(vpImagePoint top_left, unsigned int w, w_zval = w_zsize / 2.0; } - cam.initPersProjWithoutDistortion(1000, 1000, this->dWidth / 2.0, - this->dHeight / 2.0); + cam.initPersProjWithoutDistortion(1000, 1000, this->dWidth / 2.0, this->dHeight / 2.0); findPose(); @@ -218,8 +208,7 @@ void vpPlotGraph::computeGraphParameters() yorg = dTopLeft.get_i() + (ymax * zoomy); } -void vpPlotGraph::setCurveColor(const unsigned int curveNum, - const vpColor &color) +void vpPlotGraph::setCurveColor(const unsigned int curveNum, const vpColor &color) { (curveList + curveNum)->color = color; } @@ -248,15 +237,13 @@ void vpPlotGraph::setUnitZ(const std::string &unit_z) dispUnit = true; } -void vpPlotGraph::setLegend(const unsigned int curveNum, - const std::string &newlegend) +void vpPlotGraph::setLegend(const unsigned int curveNum, const std::string &newlegend) { (curveList + curveNum)->legend = newlegend; dispLegend = true; } -void vpPlotGraph::setCurveThickness(const unsigned int curveNum, - const unsigned int thickness) +void vpPlotGraph::setCurveThickness(const unsigned int curveNum, const unsigned int thickness) { (curveList + curveNum)->thickness = thickness; } @@ -301,13 +288,10 @@ void vpPlotGraph::displayGrid(vpImage &I) 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), + 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); + vpDisplay::displayDotLine(I, vpImagePoint(yorg, x), vpImagePoint(yorg - 3, x), vpColor::black, gridThickness); if (t + xdelt <= xmax + 1e-10) { double ttemp; @@ -317,28 +301,20 @@ void vpPlotGraph::displayGrid(vpImage &I) ttemp = t; sprintf(valeur, "%.2f", ttemp); #if defined VISP_HAVE_X11 - vpDisplay::displayText(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::displayText(I, vpImagePoint(yorg + epsi, x), valeur, - vpColor::black); + vpDisplay::displayText(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::displayText(I, vpImagePoint(yorg + epsi, x), valeur, vpColor::black); #endif } } if (power != 0) { sprintf(valeur, "x10e%d", -power); #if defined VISP_HAVE_X11 - vpDisplay::displayText( - 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::displayText( - I, - vpImagePoint(yorg + 4 * epsi, dTopLeft.get_j() + dWidth - 10 * epsj), - valeur, vpColor::black); + vpDisplay::displayText(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::displayText(I, vpImagePoint(yorg + 4 * epsi, dTopLeft.get_j() + dWidth - 10 * epsj), valeur, + vpColor::black); #endif } @@ -346,13 +322,10 @@ void vpPlotGraph::displayGrid(vpImage &I) 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), + 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); + vpDisplay::displayDotLine(I, vpImagePoint(y, xorg), vpImagePoint(y, xorg + 3), vpColor::black, gridThickness); double ttemp; if (power != 0) @@ -362,40 +335,28 @@ void vpPlotGraph::displayGrid(vpImage &I) sprintf(valeur, "%.2f", ttemp); #if defined VISP_HAVE_X11 - vpDisplay::displayText(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::displayText(I, vpImagePoint(y - epsi, topLeft.get_j() + epsj), - valeur, vpColor::black); + vpDisplay::displayText(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::displayText(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::displayText(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::displayText(I, - vpImagePoint(dTopLeft.get_i() - 3 * epsi, - dTopLeft.get_j() - 6 * epsj), - valeur, vpColor::black); + vpDisplay::displayText(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::displayText(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); + 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); + 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); @@ -408,35 +369,22 @@ void vpPlotGraph::displayGrid(vpImage &I) } void vpPlotGraph::displayUnit(vpImage & -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || \ +#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)unitx.size(), dWidth); + unsigned int offsetx = vpMath::minimum((unsigned int)unitx.size(), dWidth); #if defined VISP_HAVE_X11 - vpDisplay::displayText( - I, - vpImagePoint(yorg - 2 * epsi, - dTopLeft.get_j() + dWidth - offsetx * epsj), - unitx.c_str(), vpColor::black); - vpDisplay::displayText( - I, vpImagePoint(dTopLeft.get_i(), dTopLeft.get_j() + epsj), - unity.c_str(), vpColor::black); -#elif defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || \ - defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) - vpDisplay::displayText( - I, - vpImagePoint(yorg - 5 * epsi, - dTopLeft.get_j() + dWidth - offsetx * epsj), - unitx.c_str(), vpColor::black); - vpDisplay::displayText( - I, vpImagePoint(dTopLeft.get_i(), dTopLeft.get_j() + epsj), - unity.c_str(), vpColor::black); + vpDisplay::displayText(I, vpImagePoint(yorg - 2 * epsi, dTopLeft.get_j() + dWidth - offsetx * epsj), unitx.c_str(), + vpColor::black); + vpDisplay::displayText(I, vpImagePoint(dTopLeft.get_i(), dTopLeft.get_j() + epsj), unity.c_str(), vpColor::black); +#elif defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK) + vpDisplay::displayText(I, vpImagePoint(yorg - 5 * epsi, dTopLeft.get_j() + dWidth - offsetx * epsj), unitx.c_str(), + vpColor::black); + vpDisplay::displayText(I, vpImagePoint(dTopLeft.get_i(), dTopLeft.get_j() + epsj), unity.c_str(), vpColor::black); #endif } @@ -444,11 +392,8 @@ void vpPlotGraph::displayTitle(vpImage &I) { double size = (double)title.size(); size = size / 2.0; - vpDisplay::displayText( - I, - vpImagePoint(dTopLeft.get_i() - 3 * epsi, - dTopLeft.get_j() + dWidth / 2.0 - 4 * size), - title.c_str(), vpColor::black); + vpDisplay::displayText(I, vpImagePoint(dTopLeft.get_i() - 3 * epsi, dTopLeft.get_j() + dWidth / 2.0 - 4 * size), + title.c_str(), vpColor::black); } void vpPlotGraph::displayLegend(vpImage &I) @@ -462,11 +407,8 @@ void vpPlotGraph::displayLegend(vpImage &I) offsetj = dWidth; for (unsigned int i = 0; i < curveNbr; i++) { - vpDisplay::displayText(I, - vpImagePoint(dTopLeft.get_i() + i * 5 * epsi, - dTopLeft.get_j() + dWidth - offsetj), - (curveList + i)->legend.c_str(), - (curveList + i)->color); + vpDisplay::displayText(I, vpImagePoint(dTopLeft.get_i() + i * 5 * epsi, dTopLeft.get_j() + dWidth - offsetj), + (curveList + i)->legend.c_str(), (curveList + i)->color); } } @@ -498,10 +440,8 @@ void vpPlotGraph::rescaley(unsigned int side, double extremity) ydelt = (ymax - ymin) / (double)nbDivisiony; } -void vpPlotGraph::initScale(vpImage &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) +void vpPlotGraph::initScale(vpImage &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; @@ -518,12 +458,9 @@ void vpPlotGraph::initScale(vpImage &I, const double x_min, scaleInitialized = true; } -void vpPlotGraph::initScale(vpImage &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) +void vpPlotGraph::initScale(vpImage &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; @@ -543,8 +480,7 @@ void vpPlotGraph::initScale(vpImage &I, const double x_min, scaleInitialized = true; } -void vpPlotGraph::plot(vpImage &I, const unsigned int curveNb, - const double x, const double y) +void vpPlotGraph::plot(vpImage &I, const unsigned int curveNb, const double x, const double y) { if (!scaleInitialized) { if (x < 0) { @@ -621,10 +557,7 @@ void vpPlotGraph::replot(vpImage &I) vpDisplay::flushROI(I, graphZone); } -void vpPlotGraph::clearGraphZone(vpImage &I) -{ - vpDisplay::displayROI(I, graphZone); -} +void vpPlotGraph::clearGraphZone(vpImage &I) { vpDisplay::displayROI(I, graphZone); } bool vpPlotGraph::getPixelValue(vpImage &I, vpImagePoint &iP) { @@ -632,25 +565,16 @@ bool vpPlotGraph::getPixelValue(vpImage &I, vpImagePoint &iP) 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)); + 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::displayText(I, - vpImagePoint(topLeft.get_i() + height - 2, - topLeft.get_j() + 5 * epsj), - valeur, vpColor::black); + vpDisplay::displayText(I, vpImagePoint(topLeft.get_i() + height - 2, topLeft.get_j() + 5 * epsj), valeur, + vpColor::black); sprintf(valeur, " y: %f", y); - vpDisplay::displayText(I, - vpImagePoint(topLeft.get_i() + height - 2, - topLeft.get_j() + width / 2.0), - valeur, vpColor::black); + vpDisplay::displayText(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)); + vpDisplay::flushROI(I, vpRect(vpImagePoint(topLeft.get_i() + height - 20, topLeft.get_j()), width - 1, 19)); return true; } return false; @@ -794,8 +718,7 @@ bool vpPlotGraph::check3Dline(vpImagePoint &iP1, vpImagePoint &iP2) 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())) + 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())) { @@ -806,8 +729,7 @@ bool vpPlotGraph::check3Dline(vpImagePoint &iP1, vpImagePoint &iP2) iP2 = iP[0]; } } else { - if (vpMath::sign(iP1.get_i() - iP[0].get_i()) == - vpMath::sign(iP2.get_i() - iP[0].get_i())) + 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())) { @@ -915,8 +837,7 @@ void vpPlotGraph::computeGraphParameters3D() ptZorg = w_zval - zoomz_3D * zmax; } -void getGrid3DPoint(const double pente, vpImagePoint &iPunit, - vpImagePoint &ip1, vpImagePoint &ip2, vpImagePoint &ip3) +void getGrid3DPoint(const double pente, vpImagePoint &iPunit, vpImagePoint &ip1, vpImagePoint &ip2, vpImagePoint &ip3) { if (pente <= 1) { ip1 = iPunit - vpImagePoint(3, 0); @@ -949,8 +870,7 @@ void vpPlotGraph::displayGrid3D(vpImage &I) 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); + vpMeterPixelConversion::convertPoint(cam, pt[i].get_x(), pt[i].get_y(), u, v); iP[i].set_uv(u, v); iP[i] = iP[i] + dTopLeft3D; } @@ -970,10 +890,8 @@ void vpPlotGraph::displayGrid3D(vpImage &I) 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::epsilon()) - pente = fabs((iP[0].get_i() - iP[1].get_i()) / - (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::epsilon()) + pente = fabs((iP[0].get_i() - iP[1].get_i()) / (iP[0].get_j() - iP[1].get_j())); else pente = 2; @@ -983,8 +901,7 @@ void vpPlotGraph::displayGrid3D(vpImage &I) 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); + vpMeterPixelConversion::convertPoint(cam, ptunit.get_x(), ptunit.get_y(), u, v); iPunit.set_uv(u, v); iPunit = iPunit + dTopLeft3D; @@ -1015,10 +932,8 @@ void vpPlotGraph::displayGrid3D(vpImage &I) 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::epsilon()) - pente = fabs((iP[2].get_i() - iP[3].get_i()) / - (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::epsilon()) + pente = fabs((iP[2].get_i() - iP[3].get_i()) / (iP[2].get_j() - iP[3].get_j())); else pente = 2; count = 0; @@ -1027,8 +942,7 @@ void vpPlotGraph::displayGrid3D(vpImage &I) 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); + vpMeterPixelConversion::convertPoint(cam, ptunit.get_x(), ptunit.get_y(), u, v); iPunit.set_uv(u, v); iPunit = iPunit + dTopLeft3D; @@ -1059,10 +973,8 @@ void vpPlotGraph::displayGrid3D(vpImage &I) 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::epsilon()) - pente = fabs((iP[4].get_i() - iP[5].get_i()) / - (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::epsilon()) + pente = fabs((iP[4].get_i() - iP[5].get_i()) / (iP[4].get_j() - iP[5].get_j())); else pente = 2; count = 0; @@ -1071,8 +983,7 @@ void vpPlotGraph::displayGrid3D(vpImage &I) 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); + vpMeterPixelConversion::convertPoint(cam, ptunit.get_x(), ptunit.get_y(), u, v); iPunit.set_uv(u, v); iPunit = iPunit + dTopLeft3D; @@ -1131,9 +1042,8 @@ void vpPlotGraph::displayGrid3D(vpImage &I) displayLegend(I); } -vpMouseButton::vpMouseButtonType -vpPlotGraph::plot(vpImage &I, const unsigned int curveNb, - const double x, const double y, const double z) +vpMouseButton::vpMouseButtonType vpPlotGraph::plot(vpImage &I, const unsigned int curveNb, + const double x, const double y, const double z) { if (!scaleInitialized) { if (x < 0) { @@ -1210,21 +1120,17 @@ vpPlotGraph::plot(vpImage &I, const unsigned int curveNb, } vpPoint pointPlot; - pointPlot.setWorldCoordinates(ptXorg + (zoomx_3D * x), - ptYorg - (zoomy_3D * y), - ptZorg + (zoomz_3D * z)); + 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); + vpMeterPixelConversion::convertPoint(cam, pointPlot.get_x(), pointPlot.get_y(), u, v); vpImagePoint iP; iP.set_uv(u, v); iP = iP + dTopLeft3D; if ((curveList + curveNb)->nbPoint) { if (check3Dline((curveList + curveNb)->lastPoint, iP)) - vpDisplay::displayLine(I, (curveList + curveNb)->lastPoint, iP, - (curveList + curveNb)->color, + vpDisplay::displayLine(I, (curveList + curveNb)->lastPoint, iP, (curveList + curveNb)->color, (curveList + curveNb)->thickness); } #if (defined VISP_HAVE_X11 || defined VISP_HAVE_GDI) @@ -1268,12 +1174,9 @@ void vpPlotGraph::replot3D(vpImage &I) displayGrid3D(I); for (unsigned int i = 0; i < curveNbr; i++) { - std::list::const_iterator it_ptListx = - (curveList + i)->pointListx.begin(); - std::list::const_iterator it_ptListy = - (curveList + i)->pointListy.begin(); - std::list::const_iterator it_ptListz = - (curveList + i)->pointListz.begin(); + std::list::const_iterator it_ptListx = (curveList + i)->pointListx.begin(); + std::list::const_iterator it_ptListy = (curveList + i)->pointListy.begin(); + std::list::const_iterator it_ptListz = (curveList + i)->pointListz.begin(); unsigned int k = 0; vpImagePoint iP; @@ -1282,21 +1185,17 @@ void vpPlotGraph::replot3D(vpImage &I) double x = *it_ptListx; double y = *it_ptListy; double z = *it_ptListz; - pointPlot.setWorldCoordinates(ptXorg + (zoomx_3D * x), - ptYorg - (zoomy_3D * y), - ptZorg + (zoomz_3D * z)); + 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); + 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::displayLine(I, (curveList + i)->lastPoint, iP, (curveList + i)->color); // vpDisplay::displayCross(I,iP,3,vpColor::orange); } @@ -1329,8 +1228,7 @@ void vpPlotGraph::rescalez(unsigned int side, double extremity) \param I : Image used to display the graph. \param button : Mouse button used during navigation. */ -bool vpPlotGraph::move(const vpImage &I, - vpMouseButton::vpMouseButtonType &button) +bool vpPlotGraph::move(const vpImage &I, vpMouseButton::vpMouseButtonType &button) { bool changed = false; vpHomogeneousMatrix displacement = navigation(I, changed, button); @@ -1344,9 +1242,8 @@ bool vpPlotGraph::move(const vpImage &I, return changed; } -vpHomogeneousMatrix -vpPlotGraph::navigation(const vpImage &I, bool &changed, - vpMouseButton::vpMouseButtonType &b) +vpHomogeneousMatrix vpPlotGraph::navigation(const vpImage &I, bool &changed, + vpMouseButton::vpMouseButtonType &b) { vpImagePoint iP; vpImagePoint trash; diff --git a/modules/gui/test/display/testClick.cpp b/modules/gui/test/display/testClick.cpp index 3732a8cd96..358286a49d 100644 --- a/modules/gui/test/display/testClick.cpp +++ b/modules/gui/test/display/testClick.cpp @@ -43,8 +43,7 @@ #include #include -#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || \ +#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || \ defined(VISP_HAVE_OPENCV)) #include @@ -70,10 +69,8 @@ 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, +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); /*! @@ -86,8 +83,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, \param dtype : Type of video device. */ -void usage(const char *name, const char *badparam, std::string ipath, - vpDisplayType &dtype) +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\ @@ -170,8 +166,7 @@ OPTIONS: Default\n\ \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 getOptions(int argc, const char **argv, std::string &ipath, vpDisplayType &dtype, bool &list, bool &click_allowed, bool &display) { const char *optarg_; @@ -265,8 +260,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_dtype, opt_list, - opt_click_allowed, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_dtype, opt_list, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -310,8 +304,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -320,13 +313,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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); } @@ -348,8 +338,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -359,8 +348,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -370,8 +358,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -381,20 +368,17 @@ int main(int argc, const char **argv) 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"; + 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; + 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; diff --git a/modules/gui/test/display/testDisplayPolygonLines.cpp b/modules/gui/test/display/testDisplayPolygonLines.cpp index d4d163afd5..8216f1cfc6 100644 --- a/modules/gui/test/display/testDisplayPolygonLines.cpp +++ b/modules/gui/test/display/testDisplayPolygonLines.cpp @@ -56,8 +56,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! Print the program options. @@ -107,8 +106,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -173,27 +171,20 @@ int main(int argc, const char **argv) d.init(I, 0, 0, "Grayscale image"); vpDisplay::display(I); - vpDisplay::displayText( - I, 20, 20, - "Left click to draw a polygon, right click when it is finished.", - vpColor::red); + vpDisplay::displayText(I, 20, 20, "Left click to draw a polygon, right click when it is finished.", vpColor::red); vpDisplay::flush(I); vpPolygon polygon; polygon.initClick(I); vpDisplay::display(I); - vpDisplay::displayText( - I, 20, 20, "Shape is not closed. Click to display dashed lines.", - vpColor::red); + vpDisplay::displayText(I, 20, 20, "Shape is not closed. Click to display dashed lines.", vpColor::red); vpDisplay::displayLine(I, polygon.getCorners(), false, vpColor::red, 2); vpDisplay::flush(I); vpDisplay::getClick(I); vpDisplay::display(I); - vpDisplay::displayText(I, 20, 20, - "Shape is closed. Click to draw on color image.", - vpColor::red); + vpDisplay::displayText(I, 20, 20, "Shape is closed. Click to draw on color image.", vpColor::red); vpDisplay::displayDotLine(I, polygon.getCorners(), true, vpColor::red, 2); vpDisplay::flush(I); vpDisplay::getClick(I); @@ -201,8 +192,7 @@ int main(int argc, const char **argv) d2.init(I_color, I.getWidth(), 0, "Color image"); // Create colormap for (unsigned int i = 0; i < I_color.getHeight(); i++) { - double hue = i / (double)I_color.getHeight(), saturation = 1.0, - value = 1.0; + double hue = i / (double)I_color.getHeight(), saturation = 1.0, value = 1.0; unsigned char rgb[3]; vpImageConvert::HSVToRGB(&hue, &saturation, &value, rgb, 1); @@ -214,29 +204,21 @@ int main(int argc, const char **argv) } vpDisplay::display(I_color); - vpDisplay::displayText( - I_color, 20, 20, - "Left click to draw a polygon, right click when it is finished.", - vpColor::black); + vpDisplay::displayText(I_color, 20, 20, "Left click to draw a polygon, right click when it is finished.", + vpColor::black); vpDisplay::flush(I_color); polygon.initClick(I_color); vpDisplay::display(I_color); - vpDisplay::displayText(I_color, 20, 20, - "Shape is closed. Click to display dashed lines.", - vpColor::black); - vpDisplay::displayLine(I_color, polygon.getCorners(), true, vpColor::red, - 2); + vpDisplay::displayText(I_color, 20, 20, "Shape is closed. Click to display dashed lines.", vpColor::black); + vpDisplay::displayLine(I_color, polygon.getCorners(), true, vpColor::red, 2); vpDisplay::flush(I_color); vpDisplay::getClick(I_color); vpDisplay::display(I_color); - vpDisplay::displayText(I_color, 20, 20, - "Shape is not closed. Click to quit.", - vpColor::black); - vpDisplay::displayDotLine(I_color, polygon.getCorners(), false, - vpColor::red, 2); + vpDisplay::displayText(I_color, 20, 20, "Shape is not closed. Click to quit.", vpColor::black); + vpDisplay::displayDotLine(I_color, polygon.getCorners(), false, vpColor::red, 2); vpDisplay::flush(I_color); vpDisplay::getClick(I_color); } diff --git a/modules/gui/test/display/testDisplayRoi.cpp b/modules/gui/test/display/testDisplayRoi.cpp index eaf9e73c08..46522697c4 100644 --- a/modules/gui/test/display/testDisplayRoi.cpp +++ b/modules/gui/test/display/testDisplayRoi.cpp @@ -58,8 +58,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -114,8 +113,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -183,8 +181,7 @@ int main(int argc, const char **argv) I = 0; - vpRect roi(I.getWidth() / 4, I.getHeight() / 4, I.getWidth() / 2, - I.getHeight() / 2); + 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) { diff --git a/modules/gui/test/display/testDisplayScaled.cpp b/modules/gui/test/display/testDisplayScaled.cpp index 623261f936..b9aebd9b22 100644 --- a/modules/gui/test/display/testDisplayScaled.cpp +++ b/modules/gui/test/display/testDisplayScaled.cpp @@ -9,9 +9,7 @@ #include #include -template -bool test(const std::string &display, vpImage &I, unsigned int scale, - bool click) +template bool test(const std::string &display, vpImage &I, unsigned int scale, bool click) { bool success = true; unsigned int radius(I.getHeight() / 4); @@ -51,16 +49,14 @@ bool test(const std::string &display, vpImage &I, unsigned int scale, #endif } std::cout << "Start test for " << display << " renderer..." << std::endl; - std::cout << " Screen resolution: " << d->getScreenWidth() << " " - << d->getScreenHeight() << std::endl; + std::cout << " Screen resolution: " << d->getScreenWidth() << " " << d->getScreenHeight() << std::endl; d->setDownScalingFactor(scale); d->init(I); vpDisplay::display(I); vpDisplay::flush(I); vpImage crop; - vpImageTools::crop(I, vpImagePoint(0, 245), (unsigned int)roi.getHeight(), - (unsigned int)roi.getWidth(), crop); + vpImageTools::crop(I, vpImagePoint(0, 245), (unsigned int)roi.getHeight(), (unsigned int)roi.getWidth(), crop); I.insert(crop, roi.getTopLeft()); vpDisplay::displayROI(I, roi); vpDisplay::flush(I); @@ -79,8 +75,7 @@ bool test(const std::string &display, vpImage &I, unsigned int scale, if (Isampled != Irendered) { success = false; - std::cout << " -- Test width scale= " << scale << " type= " << itype - << ": failed" << std::endl; + std::cout << " -- Test width scale= " << scale << " type= " << itype << ": failed" << std::endl; std::stringstream ss; ss << "Isampled-" << itype << "-scale-" << scale; @@ -116,8 +111,7 @@ bool test(const std::string &display, vpImage &I, unsigned int scale, vpImageIo::write(Idiff, ss.str()); } else { - std::cout << " ++ Test width scale= " << scale << " type= " << itype - << ": succeed" << std::endl; + std::cout << " ++ Test width scale= " << scale << " type= " << itype << ": succeed" << std::endl; } } else { itype = "rgba"; @@ -133,8 +127,7 @@ bool test(const std::string &display, vpImage &I, unsigned int scale, vpImageConvert::convert(Isampled, IsampledCopy); if (IsampledCopy != Irendered) { success = false; - std::cout << " -- Test width scale= " << scale << " type= " << itype - << ": failed" << std::endl; + std::cout << " -- Test width scale= " << scale << " type= " << itype << ": failed" << std::endl; std::stringstream ss; ss << "Isampled-" << itype << "-scale-" << scale; @@ -170,51 +163,33 @@ bool test(const std::string &display, vpImage &I, unsigned int scale, vpImageIo::write(Idiff, ss.str()); } else { - std::cout << " ++ Test width scale= " << scale << " type= " << itype - << ": succeed" << std::endl; + std::cout << " ++ Test width scale= " << scale << " type= " << itype << ": succeed" << std::endl; } } - vpDisplay::displayRectangle(I, center - v_offset - h_offset, radius, radius, - vpColor::blue, false, thickness); - vpDisplay::displayRectangle(I, center, center + v_offset + h_offset, - vpColor::blue, false, thickness); - vpDisplay::displayRectangle( - I, vpRect(center - v_offset - h_offset, center + v_offset + h_offset), - vpColor::blue, false, thickness); - vpDisplay::displayRectangle(I, center - v_offset * 3. / 2 + h_offset, - radius / 2, radius / 2, vpColor::green, true); - vpDisplay::displayCircle(I, center, radius, vpColor::blue, false, - thickness); - vpDisplay::displayArrow(I, center, center - v_offset / 4 - h_offset, - vpColor::red, 10, 6, thickness); - vpDisplay::displayCross(I, center - radius / 2., radius, vpColor::green, - thickness); - vpDisplay::displayDotLine(I, center - v_offset - h_offset, center, - vpColor::cyan, thickness); - vpDisplay::displayLine(I, center + v_offset - h_offset, - center - v_offset + h_offset, vpColor::cyan, - thickness); + vpDisplay::displayRectangle(I, center - v_offset - h_offset, radius, radius, vpColor::blue, false, thickness); + vpDisplay::displayRectangle(I, center, center + v_offset + h_offset, vpColor::blue, false, thickness); + vpDisplay::displayRectangle(I, vpRect(center - v_offset - h_offset, center + v_offset + h_offset), vpColor::blue, + false, thickness); + vpDisplay::displayRectangle(I, center - v_offset * 3. / 2 + h_offset, radius / 2, radius / 2, vpColor::green, true); + vpDisplay::displayCircle(I, center, radius, vpColor::blue, false, thickness); + vpDisplay::displayArrow(I, center, center - v_offset / 4 - h_offset, vpColor::red, 10, 6, thickness); + vpDisplay::displayCross(I, center - radius / 2., radius, vpColor::green, thickness); + vpDisplay::displayDotLine(I, center - v_offset - h_offset, center, vpColor::cyan, thickness); + vpDisplay::displayLine(I, center + v_offset - h_offset, center - v_offset + h_offset, vpColor::cyan, thickness); int nbpoints = (int)(radius * sqrt(2.) / 8 / scale); for (int i = 0; i < nbpoints; i++) { - vpDisplay::displayPoint(I, - center - h_offset / 2. + - vpImagePoint(-i * radius_ / (nbpoints * 2), - i * radius_ / (nbpoints * 2)), - vpColor::cyan); vpDisplay::displayPoint( - I, - center - h_offset + - vpImagePoint(-i * radius_ / nbpoints, i * radius_ / nbpoints), - vpColor::cyan, thickness); + I, center - h_offset / 2. + vpImagePoint(-i * radius_ / (nbpoints * 2), i * radius_ / (nbpoints * 2)), + vpColor::cyan); + vpDisplay::displayPoint(I, center - h_offset + vpImagePoint(-i * radius_ / nbpoints, i * radius_ / nbpoints), + vpColor::cyan, thickness); } if (click) - vpDisplay::displayText(I, 10 * scale_, 10 * scale_, "A click to continue", - vpColor::red); + vpDisplay::displayText(I, 10 * scale_, 10 * scale_, "A click to continue", vpColor::red); else - vpDisplay::displayText(I, 10 * scale_, 10 * scale_, "This is an image", - vpColor::red); + vpDisplay::displayText(I, 10 * scale_, 10 * scale_, "This is an image", vpColor::red); vpDisplay::flush(I); @@ -263,11 +238,8 @@ int main(int argc, const char *argv[]) opt_display = false; else if (std::string(argv[i]) == "-i") opt_ipath = std::string(argv[i + 1]); - else if (std::string(argv[i]) == "--help" || - std::string(argv[i]) == "-h") { - std::cout << "\nUsage: " << argv[0] - << " [-i ] [-c] [-d] [--help]\n" - << std::endl; + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + std::cout << "\nUsage: " << argv[0] << " [-i ] [-c] [-d] [--help]\n" << std::endl; std::cout << "\nOptions: " << std::endl; std::cout << " -i : set image input path.\n" << " From this path read \"Klimt/Klimt.pgm\" image.\n" @@ -339,8 +311,7 @@ int main(int argc, const char *argv[]) if (nbfailure == 0) std::cout << "Test succeed" << std::endl; else - std::cout << "Test failed with " << nbfailure << " failures" - << std::endl; + std::cout << "Test failed with " << nbfailure << " failures" << std::endl; } return 0; diff --git a/modules/gui/test/display/testDisplays.cpp b/modules/gui/test/display/testDisplays.cpp index d3297605ff..a2c882c510 100644 --- a/modules/gui/test/display/testDisplays.cpp +++ b/modules/gui/test/display/testDisplays.cpp @@ -43,8 +43,7 @@ #include #include -#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || \ +#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || \ defined(VISP_HAVE_OPENCV)) #include @@ -70,8 +69,7 @@ #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); +bool getOptions(int argc, const char **argv, bool &list, bool &click_allowed, bool &display); void draw(vpImage &I); /*! @@ -122,8 +120,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, bool &list, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -256,8 +253,7 @@ int main(int argc, const char **argv) bool opt_display = true; // Read the command line options - if (getOptions(argc, argv, opt_list, opt_click_allowed, opt_display) == - false) { + if (getOptions(argc, argv, opt_list, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -399,9 +395,6 @@ int main(int argc, const char **argv) } #else -int main() -{ - std::cout << "You do not have display functionalities..." << std::endl; -} +int main() { std::cout << "You do not have display functionalities..." << std::endl; } #endif diff --git a/modules/gui/test/display/testMouseEvent.cpp b/modules/gui/test/display/testMouseEvent.cpp index d706d93279..7e78b34e97 100644 --- a/modules/gui/test/display/testMouseEvent.cpp +++ b/modules/gui/test/display/testMouseEvent.cpp @@ -56,8 +56,7 @@ #include #include -#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9)) +#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9)) #include #include @@ -89,13 +88,10 @@ typedef enum { 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); +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); /*! @@ -111,9 +107,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, \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) +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\ @@ -197,8 +192,7 @@ SYNOPSIS\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()); + 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); @@ -227,10 +221,8 @@ SYNOPSIS\n\ \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) +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; @@ -339,9 +331,8 @@ int main(int argc, const char **argv) 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) { + 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 @@ -383,27 +374,21 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " 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); + 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 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 option if you want to " - << std::endl - << " use personal images." << std::endl - << std::endl; + std::cerr << " Use -i 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 option if you want to " << std::endl + << " use personal images." << std::endl + << std::endl; exit(-1); } @@ -461,8 +446,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable" << std::endl; exit(-1); } // Create a display for the image @@ -475,8 +459,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -486,8 +469,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -497,8 +479,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -508,8 +489,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -569,8 +549,7 @@ int main(int argc, const char **argv) if (opt_click) { vpMouseButton::vpMouseButtonType button; - bool pressed = - vpDisplay::getClick(I, ip, button, opt_click_blocking); + bool pressed = vpDisplay::getClick(I, ip, button, opt_click_blocking); if (pressed) { switch (button) { case vpMouseButton::button1: @@ -610,9 +589,6 @@ int main(int argc, const char **argv) // std::cout << "Total Time : "<< tms_total< #include #include -#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || \ +#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || \ defined(VISP_HAVE_OPENCV)) #include @@ -69,10 +68,8 @@ 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, +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); /*! @@ -85,8 +82,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, \param dtype : Type of video device. */ -void usage(const char *name, const char *badparam, std::string ipath, - vpDisplayType &dtype) +void usage(const char *name, const char *badparam, std::string ipath, vpDisplayType &dtype) { fprintf(stdout, "\n\ Test video devices or display.\n\ @@ -165,8 +161,7 @@ OPTIONS: Default\n\ \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 getOptions(int argc, const char **argv, std::string &ipath, vpDisplayType &dtype, bool &list, bool &click_allowed, bool &display) { const char *optarg_; @@ -260,8 +255,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_dtype, opt_list, - opt_click_allowed, opt_display) == false) { + if (getOptions(argc, argv, opt_ipath, opt_dtype, opt_list, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -305,8 +299,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -315,13 +308,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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); } @@ -350,8 +340,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -361,8 +350,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -372,8 +360,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -383,20 +370,17 @@ int main(int argc, const char **argv) 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"; + 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; + 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; diff --git a/modules/gui/test/display/testVideoDeviceDual.cpp b/modules/gui/test/display/testVideoDeviceDual.cpp index 65a1f8306f..c22f792667 100644 --- a/modules/gui/test/display/testVideoDeviceDual.cpp +++ b/modules/gui/test/display/testVideoDeviceDual.cpp @@ -47,8 +47,7 @@ #include #include #include -#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || \ - defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || \ +#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || \ defined(VISP_HAVE_OPENCV)) /*! @@ -64,8 +63,7 @@ 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); +bool getOptions(int argc, const char **argv, vpDisplayType &dtype, bool &list, bool &click_allowed, bool &display); /*! @@ -145,8 +143,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, vpDisplayType &dtype, bool &list, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -224,8 +221,7 @@ int main(int argc, const char **argv) #endif // Read the command line options - if (getOptions(argc, argv, opt_dtype, opt_list, opt_click_allowed, - opt_display) == false) { + if (getOptions(argc, argv, opt_dtype, opt_list, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -276,8 +272,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -288,8 +283,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -300,8 +294,7 @@ int main(int argc, const char **argv) 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; @@ -312,21 +305,18 @@ int main(int argc, const char **argv) 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"; + 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; + 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"; + std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n"; return 0; #endif break; diff --git a/modules/imgproc/include/visp3/imgproc/vpContours.h b/modules/imgproc/include/visp3/imgproc/vpContours.h index 7a4c4c1177..b448c036e8 100644 --- a/modules/imgproc/include/visp3/imgproc/vpContours.h +++ b/modules/imgproc/include/visp3/imgproc/vpContours.h @@ -124,8 +124,7 @@ struct vpDirection { { vpDirection direction; int directionSize = LAST_DIRECTION; - direction.m_direction = - vpDirectionType(((int)m_direction + 1) % directionSize); + direction.m_direction = vpDirectionType(((int)m_direction + 1) % directionSize); return direction; } @@ -145,8 +144,7 @@ struct vpDirection { int yy = (int)(point.get_i() + m_diry[(int)m_direction]); int xx = (int)(point.get_j() + m_dirx[(int)m_direction]); - if (xx < 0 || xx >= (int)I.getWidth() || yy < 0 || - yy >= (int)I.getHeight()) { + if (xx < 0 || xx >= (int)I.getWidth() || yy < 0 || yy >= (int)I.getHeight()) { return vpImagePoint(-1, -1); } @@ -164,9 +162,9 @@ typedef enum { } vpContourType; typedef enum { - CONTOUR_RETR_TREE, /*!< Retrieve all the contours with the hierarchy stored - in a tree. */ - CONTOUR_RETR_LIST, /*!< Retrieve all the contours without any hierarchy. */ + CONTOUR_RETR_TREE, /*!< Retrieve all the contours with the hierarchy stored + in a tree. */ + CONTOUR_RETR_LIST, /*!< Retrieve all the contours without any hierarchy. */ CONTOUR_RETR_EXTERNAL /*!< Retrieve only external contours. */ } vpContourRetrievalType; @@ -176,26 +174,17 @@ struct vpContour { vpContour *m_parent; std::vector m_points; - vpContour() - : m_children(), m_contourType(vp::CONTOUR_HOLE), m_parent(NULL), - m_points() - { - } + vpContour() : m_children(), m_contourType(vp::CONTOUR_HOLE), m_parent(NULL), m_points() {} - vpContour(const vpContourType &type) - : m_children(), m_contourType(type), m_parent(NULL), m_points() - { - } + vpContour(const vpContourType &type) : m_children(), m_contourType(type), m_parent(NULL), m_points() {} vpContour(const vpContour &contour) - : m_children(), m_contourType(contour.m_contourType), m_parent(NULL), - m_points(contour.m_points) + : m_children(), m_contourType(contour.m_contourType), m_parent(NULL), m_points(contour.m_points) { // Copy the underlying contours - for (std::vector::const_iterator it = - contour.m_children.begin(); - it != contour.m_children.end(); ++it) { + for (std::vector::const_iterator it = contour.m_children.begin(); it != contour.m_children.end(); + ++it) { vpContour *copy = new vpContour(**it); copy->m_parent = this; m_children.push_back(copy); @@ -204,8 +193,7 @@ struct vpContour { ~vpContour() { - for (std::vector::iterator it = m_children.begin(); - it != m_children.end(); ++it) { + for (std::vector::iterator it = m_children.begin(); it != m_children.end(); ++it) { (*it)->m_parent = NULL; if (*it != NULL) { delete *it; @@ -220,8 +208,7 @@ struct vpContour { if (m_parent == NULL) { // We are a root or an unintialized contour so delete everything - for (std::vector::iterator it = m_children.begin(); - it != m_children.end(); ++it) { + for (std::vector::iterator it = m_children.begin(); it != m_children.end(); ++it) { (*it)->m_parent = NULL; if (*it != NULL) { delete *it; @@ -235,9 +222,7 @@ struct vpContour { } m_children.clear(); - for (std::vector::const_iterator it = - other.m_children.begin(); - it != other.m_children.end(); ++it) { + for (std::vector::const_iterator it = other.m_children.begin(); it != other.m_children.end(); ++it) { vpContour *copy = new vpContour(**it); copy->m_parent = this; m_children.push_back(copy); @@ -256,19 +241,14 @@ struct vpContour { } }; -VISP_EXPORT void -drawContours(vpImage &I, - const std::vector > &contours, - unsigned char grayValue = 255); -VISP_EXPORT void -drawContours(vpImage &I, - const std::vector > &contours, - const vpColor &color); - -VISP_EXPORT void findContours( - const vpImage &I_original, vpContour &contours, - std::vector > &contourPts, - const vpContourRetrievalType &retrievalMode = vp::CONTOUR_RETR_TREE); +VISP_EXPORT void drawContours(vpImage &I, const std::vector > &contours, + unsigned char grayValue = 255); +VISP_EXPORT void drawContours(vpImage &I, const std::vector > &contours, + const vpColor &color); + +VISP_EXPORT void findContours(const vpImage &I_original, vpContour &contours, + std::vector > &contourPts, + const vpContourRetrievalType &retrievalMode = vp::CONTOUR_RETR_TREE); } #endif diff --git a/modules/imgproc/include/visp3/imgproc/vpImgproc.h b/modules/imgproc/include/visp3/imgproc/vpImgproc.h index 36aa7cdbe2..b3b95917cb 100644 --- a/modules/imgproc/include/visp3/imgproc/vpImgproc.h +++ b/modules/imgproc/include/visp3/imgproc/vpImgproc.h @@ -72,119 +72,80 @@ typedef enum { histogram-based thresholding algorithms", CVGIP: Graphical Models and Image Processing 55: 532-537 \cite Glasbey:1993:AHT:167725.167747 */ - AUTO_THRESHOLD_OTSU, /*!< Otsu, N (1979), "A threshold selection method from - gray-level histograms", IEEE Trans. Sys., Man., - Cyber. 9: 62-66, doi:10.1109/TSMC.1979.4310076 - \cite article4310076 */ - AUTO_THRESHOLD_TRIANGLE /*!< Zack GW, Rogers WE, Latt SA (1977), "Automatic - measurement of sister chromatid exchange - frequency", J. Histochem. Cytochem. 25 (7): - 741–53, PMID 70454 \cite doi:10.1177/25.7.70454 - */ + AUTO_THRESHOLD_OTSU, /*!< Otsu, N (1979), "A threshold selection method from + gray-level histograms", IEEE Trans. Sys., Man., + Cyber. 9: 62-66, doi:10.1109/TSMC.1979.4310076 + \cite article4310076 */ + AUTO_THRESHOLD_TRIANGLE /*!< Zack GW, Rogers WE, Latt SA (1977), "Automatic + measurement of sister chromatid exchange + frequency", J. Histochem. Cytochem. 25 (7): + 741–53, PMID 70454 \cite doi:10.1177/25.7.70454 + */ } vpAutoThresholdMethod; -VISP_EXPORT void adjust(vpImage &I, const double alpha, +VISP_EXPORT void adjust(vpImage &I, const double alpha, const double beta); +VISP_EXPORT void adjust(const vpImage &I1, vpImage &I2, const double alpha, const double beta); -VISP_EXPORT void adjust(const vpImage &I1, - vpImage &I2, const double alpha, - const double beta); -VISP_EXPORT void adjust(vpImage &I, const double alpha, - const double beta); -VISP_EXPORT void adjust(const vpImage &I1, vpImage &I2, - const double alpha, const double beta); +VISP_EXPORT void adjust(vpImage &I, const double alpha, const double beta); +VISP_EXPORT void adjust(const vpImage &I1, vpImage &I2, const double alpha, const double beta); -VISP_EXPORT void clahe(const vpImage &I1, - vpImage &I2, - const int blockRadius = 150, const int bins = 256, - const float slope = 3.0f, const bool fast = true); -VISP_EXPORT void clahe(const vpImage &I1, vpImage &I2, - const int blockRadius = 150, const int bins = 256, - const float slope = 3.0f, const bool fast = true); +VISP_EXPORT void clahe(const vpImage &I1, vpImage &I2, const int blockRadius = 150, + const int bins = 256, const float slope = 3.0f, const bool fast = true); +VISP_EXPORT void clahe(const vpImage &I1, vpImage &I2, const int blockRadius = 150, + const int bins = 256, const float slope = 3.0f, const bool fast = true); VISP_EXPORT void equalizeHistogram(vpImage &I); -VISP_EXPORT void equalizeHistogram(const vpImage &I1, - vpImage &I2); -VISP_EXPORT void equalizeHistogram(vpImage &I, - const bool useHSV = false); -VISP_EXPORT void equalizeHistogram(const vpImage &I1, - vpImage &I2, - const bool useHSV = false); - -VISP_EXPORT void gammaCorrection(vpImage &I, - const double gamma); -VISP_EXPORT void gammaCorrection(const vpImage &I1, - vpImage &I2, - const double gamma); +VISP_EXPORT void equalizeHistogram(const vpImage &I1, vpImage &I2); +VISP_EXPORT void equalizeHistogram(vpImage &I, const bool useHSV = false); +VISP_EXPORT void equalizeHistogram(const vpImage &I1, vpImage &I2, const bool useHSV = false); + +VISP_EXPORT void gammaCorrection(vpImage &I, const double gamma); +VISP_EXPORT void gammaCorrection(const vpImage &I1, vpImage &I2, const double gamma); VISP_EXPORT void gammaCorrection(vpImage &I, const double gamma); -VISP_EXPORT void gammaCorrection(const vpImage &I1, - vpImage &I2, const double gamma); - -VISP_EXPORT void retinex(vpImage &I, const int scale = 240, - const int scaleDiv = 3, - const int level = RETINEX_UNIFORM, - const double dynamic = 1.2, - const int kernelSize = -1); -VISP_EXPORT void retinex(const vpImage &I1, vpImage &I2, - const int scale = 240, const int scaleDiv = 3, - const int level = RETINEX_UNIFORM, - const double dynamic = 1.2, - const int kernelSize = -1); +VISP_EXPORT void gammaCorrection(const vpImage &I1, vpImage &I2, const double gamma); + +VISP_EXPORT void retinex(vpImage &I, const int scale = 240, const int scaleDiv = 3, + const int level = RETINEX_UNIFORM, const double dynamic = 1.2, const int kernelSize = -1); +VISP_EXPORT void retinex(const vpImage &I1, vpImage &I2, const int scale = 240, const int scaleDiv = 3, + const int level = RETINEX_UNIFORM, const double dynamic = 1.2, const int kernelSize = -1); VISP_EXPORT void stretchContrast(vpImage &I); -VISP_EXPORT void stretchContrast(const vpImage &I1, - vpImage &I2); +VISP_EXPORT void stretchContrast(const vpImage &I1, vpImage &I2); VISP_EXPORT void stretchContrast(vpImage &I); -VISP_EXPORT void stretchContrast(const vpImage &I1, - vpImage &I2); +VISP_EXPORT void stretchContrast(const vpImage &I1, vpImage &I2); VISP_EXPORT void stretchContrastHSV(vpImage &I); -VISP_EXPORT void stretchContrastHSV(const vpImage &I1, - vpImage &I2); +VISP_EXPORT void stretchContrastHSV(const vpImage &I1, vpImage &I2); -VISP_EXPORT void unsharpMask(vpImage &I, - const unsigned int size = 7, - const double weight = 0.6); -VISP_EXPORT void unsharpMask(const vpImage &I, - vpImage &Ires, - const unsigned int size = 7, +VISP_EXPORT void unsharpMask(vpImage &I, const unsigned int size = 7, const double weight = 0.6); +VISP_EXPORT void unsharpMask(const vpImage &I, vpImage &Ires, const unsigned int size = 7, const double weight = 0.6); -VISP_EXPORT void unsharpMask(vpImage &I, const unsigned int size = 7, - const double weight = 0.6); -VISP_EXPORT void unsharpMask(const vpImage &I, vpImage &Ires, - const unsigned int size = 7, +VISP_EXPORT void unsharpMask(vpImage &I, const unsigned int size = 7, const double weight = 0.6); +VISP_EXPORT void unsharpMask(const vpImage &I, vpImage &Ires, const unsigned int size = 7, const double weight = 0.6); VISP_EXPORT void -connectedComponents(const vpImage &I, vpImage &labels, - int &nbComponents, - const vpImageMorphology::vpConnexityType &connexity = - vpImageMorphology::CONNEXITY_4); +connectedComponents(const vpImage &I, vpImage &labels, int &nbComponents, + const vpImageMorphology::vpConnexityType &connexity = vpImageMorphology::CONNEXITY_4); VISP_EXPORT void fillHoles(vpImage &I #if USE_OLD_FILL_HOLE , - const vpImageMorphology::vpConnexityType - &connexity = vpImageMorphology::CONNEXITY_4 + const vpImageMorphology::vpConnexityType &connexity = vpImageMorphology::CONNEXITY_4 #endif ); -VISP_EXPORT void -floodFill(vpImage &I, const vpImagePoint &seedPoint, - const unsigned char oldValue, const unsigned char newValue, - const vpImageMorphology::vpConnexityType &connexity = - vpImageMorphology::CONNEXITY_4); +VISP_EXPORT void floodFill(vpImage &I, const vpImagePoint &seedPoint, const unsigned char oldValue, + const unsigned char newValue, + const vpImageMorphology::vpConnexityType &connexity = vpImageMorphology::CONNEXITY_4); -VISP_EXPORT void -reconstruct(const vpImage &marker, - const vpImage &mask, vpImage &I, - const vpImageMorphology::vpConnexityType &connexity = - vpImageMorphology::CONNEXITY_4); - -VISP_EXPORT unsigned char -autoThreshold(vpImage &I, - const vp::vpAutoThresholdMethod &method, - const unsigned char backgroundValue = 0, - const unsigned char foregroundValue = 255); +VISP_EXPORT void reconstruct(const vpImage &marker, const vpImage &mask, + vpImage &I, + const vpImageMorphology::vpConnexityType &connexity = vpImageMorphology::CONNEXITY_4); + +VISP_EXPORT unsigned char autoThreshold(vpImage &I, const vp::vpAutoThresholdMethod &method, + const unsigned char backgroundValue = 0, + const unsigned char foregroundValue = 255); } #endif diff --git a/modules/imgproc/src/vpCLAHE.cpp b/modules/imgproc/src/vpCLAHE.cpp index a3ff850267..3b8727f861 100644 --- a/modules/imgproc/src/vpCLAHE.cpp +++ b/modules/imgproc/src/vpCLAHE.cpp @@ -88,8 +88,7 @@ namespace { int fastRound(const float value) { return (int)(value + 0.5f); } -void clipHistogram(const std::vector &hist, - std::vector &clippedHist, const int limit) +void clipHistogram(const std::vector &hist, std::vector &clippedHist, const int limit) { clippedHist = hist; int clippedEntries = 0, clippedEntriesBefore = 0; @@ -121,8 +120,7 @@ void clipHistogram(const std::vector &hist, } while (clippedEntries != clippedEntriesBefore); } -void createHistogram(const int blockRadius, const int bins, - const int blockXCenter, const int blockYCenter, +void createHistogram(const int blockRadius, const int bins, const int blockXCenter, const int blockYCenter, const vpImage &I, std::vector &hist) { std::fill(hist.begin(), hist.end(), 0); @@ -139,8 +137,7 @@ void createHistogram(const int blockRadius, const int bins, } } -std::vector createTransfer(const std::vector &hist, - const int limit, std::vector &cdfs) +std::vector createTransfer(const std::vector &hist, const int limit, std::vector &cdfs) { clipHistogram(hist, cdfs, limit); int hMin = (int)hist.size() - 1; @@ -191,8 +188,7 @@ float transferValue(const int v, std::vector &clippedHist) return (cdf - cdfMin) / (float)(cdfMax - cdfMin); } -float transferValue(const int v, const std::vector &hist, - std::vector &clippedHist, const int limit) +float transferValue(const int v, const std::vector &hist, std::vector &clippedHist, const int limit) { clipHistogram(hist, clippedHist, limit); @@ -225,9 +221,8 @@ float transferValue(const int v, const std::vector &hist, boxes of the given block size only and interpolates for locations in between. */ -void vp::clahe(const vpImage &I1, vpImage &I2, - const int blockRadius, const int bins, const float slope, - const bool fast) +void vp::clahe(const vpImage &I1, vpImage &I2, const int blockRadius, const int bins, + const float slope, const bool fast) { if (blockRadius < 0) { std::cerr << "Error: blockRadius < 0!" << std::endl; @@ -239,8 +234,7 @@ void vp::clahe(const vpImage &I1, vpImage &I2, return; } - if ((unsigned int)(2 * blockRadius + 1) > I1.getWidth() || - (unsigned int)(2 * blockRadius + 1) > I1.getHeight()) { + if ((unsigned int)(2 * blockRadius + 1) > I1.getWidth() || (unsigned int)(2 * blockRadius + 1) > I1.getHeight()) { std::cerr << "Error: (unsigned int) (2*blockRadius+1) > I1.getWidth() || " "(unsigned int) (2*blockRadius+1) > I1.getHeight()!" << std::endl; @@ -461,8 +455,7 @@ void vp::clahe(const vpImage &I1, vpImage &I2, int w = std::min((int)I1.getWidth(), xMax) - xMin; int n = h * w; int limit = (int)(slope * n / bins + 0.5f); - I2[y][x] = - fastRound(transferValue(v, hist, clippedHist, limit) * 255.0f); + I2[y][x] = fastRound(transferValue(v, hist, clippedHist, limit) * 255.0f); } } } @@ -493,8 +486,7 @@ void vp::clahe(const vpImage &I1, vpImage &I2, boxes of the given block size only and interpolates for locations in between. */ -void vp::clahe(const vpImage &I1, vpImage &I2, - const int blockRadius, const int bins, const float slope, +void vp::clahe(const vpImage &I1, vpImage &I2, const int blockRadius, const int bins, const float slope, const bool fast) { // Split diff --git a/modules/imgproc/src/vpConnectedComponents.cpp b/modules/imgproc/src/vpConnectedComponents.cpp index 013e588c86..20eb5ca3a9 100644 --- a/modules/imgproc/src/vpConnectedComponents.cpp +++ b/modules/imgproc/src/vpConnectedComponents.cpp @@ -46,10 +46,8 @@ namespace { -void getNeighbors(const vpImage &I, - std::queue &listOfNeighbors, - const unsigned int i, const unsigned int j, - const vpImageMorphology::vpConnexityType &connexity) +void getNeighbors(const vpImage &I, std::queue &listOfNeighbors, const unsigned int i, + const unsigned int j, const vpImageMorphology::vpConnexityType &connexity) { unsigned char currValue = I[i][j]; @@ -87,10 +85,8 @@ void getNeighbors(const vpImage &I, } } -void visitNeighbors(vpImage &I_copy, - std::queue &listOfNeighbors, - vpImage &labels, const int current_label, - const vpImageMorphology::vpConnexityType &connexity) +void visitNeighbors(vpImage &I_copy, std::queue &listOfNeighbors, vpImage &labels, + const int current_label, const vpImageMorphology::vpConnexityType &connexity) { // Visit the neighbors while (!listOfNeighbors.empty()) { @@ -120,9 +116,8 @@ void visitNeighbors(vpImage &I_copy, label. \param nbComponents : Number of connected components. \param connexity : Type of connexity. */ -void vp::connectedComponents( - const vpImage &I, vpImage &labels, int &nbComponents, - const vpImageMorphology::vpConnexityType &connexity) +void vp::connectedComponents(const vpImage &I, vpImage &labels, int &nbComponents, + const vpImageMorphology::vpConnexityType &connexity) { if (I.getSize() == 0) { return; @@ -161,8 +156,7 @@ void vp::connectedComponents( I_copy[i][j] = 0; labels_copy[i][j] = current_label; - visitNeighbors(I_copy, listOfNeighbors, labels_copy, current_label, - connexity); + visitNeighbors(I_copy, listOfNeighbors, labels_copy, current_label, connexity); // Increment label current_label++; @@ -171,8 +165,7 @@ void vp::connectedComponents( } for (unsigned int i = 0; i < labels.getHeight(); i++) { - memcpy(labels[i], labels_copy[i + 1] + 1, - sizeof(int) * labels.getWidth()); + memcpy(labels[i], labels_copy[i + 1] + 1, sizeof(int) * labels.getWidth()); } nbComponents = current_label - 1; diff --git a/modules/imgproc/src/vpContours.cpp b/modules/imgproc/src/vpContours.cpp index 7439ddf70f..c8f94dc779 100644 --- a/modules/imgproc/src/vpContours.cpp +++ b/modules/imgproc/src/vpContours.cpp @@ -77,23 +77,20 @@ namespace { -bool fromTo(const vpImagePoint &from, const vpImagePoint &to, - vpDirection &direction) +bool fromTo(const vpImagePoint &from, const vpImagePoint &to, vpDirection &direction) { if (from == to) { return false; } - if (std::fabs(from.get_i() - to.get_i()) < - std::numeric_limits::epsilon()) { + if (std::fabs(from.get_i() - to.get_i()) < std::numeric_limits::epsilon()) { if (from.get_j() < to.get_j()) { direction.m_direction = EAST; } else { direction.m_direction = WEST; } } else if (from.get_i() < to.get_i()) { - if (std::fabs(from.get_j() - to.get_j()) < - std::numeric_limits::epsilon()) { + if (std::fabs(from.get_j() - to.get_j()) < std::numeric_limits::epsilon()) { direction.m_direction = SOUTH; } else if (from.get_j() < to.get_j()) { direction.m_direction = SOUTH_EAST; @@ -101,8 +98,7 @@ bool fromTo(const vpImagePoint &from, const vpImagePoint &to, direction.m_direction = SOUTH_WEST; } } else { - if (std::fabs(from.get_j() - to.get_j()) < - std::numeric_limits::epsilon()) { + if (std::fabs(from.get_j() - to.get_j()) < std::numeric_limits::epsilon()) { direction.m_direction = NORTH; } else if (from.get_j() < to.get_j()) { direction.m_direction = NORTH_EAST; @@ -114,12 +110,10 @@ bool fromTo(const vpImagePoint &from, const vpImagePoint &to, return true; } -bool crossesEastBorder(const vpImage &I, bool checked[8], - const vpImagePoint &point) +bool crossesEastBorder(const vpImage &I, bool checked[8], const vpImagePoint &point) { vpDirection direction; - if (!fromTo(point, vpImagePoint(point.get_i(), point.get_j() + 1), - direction)) { + if (!fromTo(point, vpImagePoint(point.get_i(), point.get_j() + 1), direction)) { return false; } @@ -132,16 +126,12 @@ bool crossesEastBorder(const vpImage &I, bool checked[8], unsigned int i = (unsigned int)point.get_i(); unsigned int j = (unsigned int)point.get_j(); - return I[i][j] != 0 && - ((unsigned int)point.get_j() == I.getWidth() - 1 || b); + return I[i][j] != 0 && ((unsigned int)point.get_j() == I.getWidth() - 1 || b); } -void addContourPoint(vpImage &I, vp::vpContour *border, - const vpImagePoint &point, bool checked[8], - const int nbd) +void addContourPoint(vpImage &I, vp::vpContour *border, const vpImagePoint &point, bool checked[8], const int nbd) { - border->m_points.push_back(vpImagePoint( - point.get_i() - 1, point.get_j() - 1)); // remove 1-pixel padding + border->m_points.push_back(vpImagePoint(point.get_i() - 1, point.get_j() - 1)); // remove 1-pixel padding unsigned int i = (unsigned int)point.get_i(); unsigned int j = (unsigned int)point.get_j(); @@ -154,8 +144,7 @@ void addContourPoint(vpImage &I, vp::vpContour *border, } // Otherwise leave it alone } -void followBorder(vpImage &I, const vpImagePoint &ij, vpImagePoint &i2j2, - vp::vpContour *border, const int nbd) +void followBorder(vpImage &I, const vpImagePoint &ij, vpImagePoint &i2j2, vp::vpContour *border, const int nbd) { vpDirection dir; if (!fromTo(ij, i2j2, dir)) { @@ -233,8 +222,7 @@ bool isHoleBorderStart(const vpImage &I, unsigned int i, unsigned int j) return (I[i][j] >= 1 && (j == I.getWidth() - 1 || I[i][j + 1] == 0)); } -void getContoursList(const vp::vpContour &root, const int level, - vp::vpContour &contour_list) +void getContoursList(const vp::vpContour &root, const int level, vp::vpContour &contour_list) { if (level > 0) { vp::vpContour *contour_node = new vp::vpContour; @@ -244,9 +232,7 @@ void getContoursList(const vp::vpContour &root, const int level, contour_list.m_children.push_back(contour_node); } - for (std::vector::const_iterator it = - root.m_children.begin(); - it != root.m_children.end(); ++it) { + for (std::vector::const_iterator it = root.m_children.begin(); it != root.m_children.end(); ++it) { getContoursList(**it, level + 1, contour_list); } } @@ -261,19 +247,15 @@ void getContoursList(const vp::vpContour &root, const int level, \param contours : Detected contours. \param grayValue : Drawing grayscale color. */ -void vp::drawContours(vpImage &I, - const std::vector > &contours, +void vp::drawContours(vpImage &I, const std::vector > &contours, unsigned char grayValue) { if (I.getSize() == 0) { return; } - for (std::vector >::const_iterator it1 = - contours.begin(); - it1 != contours.end(); ++it1) { - for (std::vector::const_iterator it2 = it1->begin(); - it2 != it1->end(); ++it2) { + for (std::vector >::const_iterator it1 = contours.begin(); it1 != contours.end(); ++it1) { + for (std::vector::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) { unsigned int i = (unsigned int)it2->get_i(); unsigned int j = (unsigned int)it2->get_j(); I[i][j] = grayValue; @@ -290,19 +272,14 @@ void vp::drawContours(vpImage &I, \param contours : Detected contours. \param color : Drawing color. */ -void vp::drawContours(vpImage &I, - const std::vector > &contours, - const vpColor &color) +void vp::drawContours(vpImage &I, const std::vector > &contours, const vpColor &color) { if (I.getSize() == 0) { return; } - for (std::vector >::const_iterator it1 = - contours.begin(); - it1 != contours.end(); ++it1) { - for (std::vector::const_iterator it2 = it1->begin(); - it2 != it1->end(); ++it2) { + for (std::vector >::const_iterator it1 = contours.begin(); it1 != contours.end(); ++it1) { + for (std::vector::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) { unsigned int i = (unsigned int)it2->get_i(); unsigned int j = (unsigned int)it2->get_j(); I[i][j] = vpRGBa(color.R, color.G, color.B); @@ -320,10 +297,8 @@ void vp::drawContours(vpImage &I, contours. \param contourPts : List of contours, each contour contains a list of contour points. \param retrievalMode : Contour retrieval mode. */ -void vp::findContours(const vpImage &I_original, - vpContour &contours, - std::vector > &contourPts, - const vpContourRetrievalType &retrievalMode) +void vp::findContours(const vpImage &I_original, vpContour &contours, + std::vector > &contourPts, const vpContourRetrievalType &retrievalMode) { if (I_original.getSize() == 0) { return; @@ -425,13 +400,11 @@ void vp::findContours(const vpImage &I_original, //(3) (1) ; single pixel contour if (border->m_points.empty()) { - border->m_points.push_back(vpImagePoint( - ij.get_i() - 1, ij.get_j() - 1)); // remove 1-pixel padding + border->m_points.push_back(vpImagePoint(ij.get_i() - 1, ij.get_j() - 1)); // remove 1-pixel padding I[i][j] = -nbd; } - if (retrievalMode == CONTOUR_RETR_LIST || - retrievalMode == CONTOUR_RETR_TREE) { + if (retrievalMode == CONTOUR_RETR_LIST || retrievalMode == CONTOUR_RETR_TREE) { // Add contour points contourPts.push_back(border->m_points); } @@ -446,13 +419,11 @@ void vp::findContours(const vpImage &I_original, } } - if (retrievalMode == CONTOUR_RETR_EXTERNAL || - retrievalMode == CONTOUR_RETR_LIST) { + if (retrievalMode == CONTOUR_RETR_EXTERNAL || retrievalMode == CONTOUR_RETR_LIST) { // Delete contours content contours.m_parent = NULL; - for (std::vector::iterator it = contours.m_children.begin(); - it != contours.m_children.end(); ++it) { + for (std::vector::iterator it = contours.m_children.begin(); it != contours.m_children.end(); ++it) { (*it)->m_parent = NULL; if (*it != NULL) { delete *it; @@ -465,9 +436,7 @@ void vp::findContours(const vpImage &I_original, if (retrievalMode == CONTOUR_RETR_EXTERNAL) { // Add only external contours - for (std::vector::const_iterator it = - root->m_children.begin(); - it != root->m_children.end(); ++it) { + for (std::vector::const_iterator it = root->m_children.begin(); it != root->m_children.end(); ++it) { // Save children std::vector children_copy = (*it)->m_children; // Erase children @@ -486,8 +455,7 @@ void vp::findContours(const vpImage &I_original, getContoursList(*root, 0, contours); // Set parent to root - for (std::vector::iterator it = contours.m_children.begin(); - it != contours.m_children.end(); ++it) { + for (std::vector::iterator it = contours.m_children.begin(); it != contours.m_children.end(); ++it) { (*it)->m_parent = &contours; } } else { diff --git a/modules/imgproc/src/vpFloodFill.cpp b/modules/imgproc/src/vpFloodFill.cpp index de5ceeb093..52d5b3dcbf 100644 --- a/modules/imgproc/src/vpFloodFill.cpp +++ b/modules/imgproc/src/vpFloodFill.cpp @@ -82,9 +82,8 @@ \param newValue : New value to flood fill. \param connexity : Type of connexity. */ -void vp::floodFill(vpImage &I, const vpImagePoint &seedPoint, - const unsigned char oldValue, const unsigned char newValue, - const vpImageMorphology::vpConnexityType &connexity) +void vp::floodFill(vpImage &I, const vpImagePoint &seedPoint, const unsigned char oldValue, + const unsigned char newValue, const vpImageMorphology::vpConnexityType &connexity) { // Code from Lode Vandevenne tutorial. // Naive modification for 8-connexity implementation @@ -158,8 +157,7 @@ void vp::floodFill(vpImage &I, const vpImagePoint &seedPoint, spanBelow = true; } } - } else if (spanBelow && y < I.getHeight() - 1 && - I[y + 1][x1] != oldValue) { + } else if (spanBelow && y < I.getHeight() - 1 && I[y + 1][x1] != oldValue) { spanBelow = false; } diff --git a/modules/imgproc/src/vpImgproc.cpp b/modules/imgproc/src/vpImgproc.cpp index 93f12f650c..36c4425abd 100644 --- a/modules/imgproc/src/vpImgproc.cpp +++ b/modules/imgproc/src/vpImgproc.cpp @@ -77,8 +77,7 @@ \param alpha : Multiplication coefficient. \param beta : Constant value added to the old intensity. */ -void vp::adjust(vpImage &I, const double alpha, - const double beta) +void vp::adjust(vpImage &I, const double alpha, const double beta) { // Construct the look-up table unsigned char lut[256]; @@ -101,8 +100,7 @@ void vp::adjust(vpImage &I, const double alpha, \param alpha : Multiplication coefficient. \param beta : Constant value added to the old intensity. */ -void vp::adjust(const vpImage &I1, vpImage &I2, - const double alpha, const double beta) +void vp::adjust(const vpImage &I1, vpImage &I2, const double alpha, const double beta) { // Copy I1 to I2 I2 = I1; @@ -146,8 +144,7 @@ void vp::adjust(vpImage &I, const double alpha, const double beta) \param alpha : Multiplication coefficient. \param beta : Constant value added to the old intensity. */ -void vp::adjust(const vpImage &I1, vpImage &I2, - const double alpha, const double beta) +void vp::adjust(const vpImage &I1, vpImage &I2, const double alpha, const double beta) { // Copy I1 to I2 I2 = I1; @@ -176,8 +173,7 @@ void vp::equalizeHistogram(vpImage &I) // Calculate the cumulative distribution function unsigned int cdf[256]; - unsigned int cdfMin = /*std::numeric_limits::max()*/ UINT_MAX, - cdfMax = 0; + unsigned int cdfMin = /*std::numeric_limits::max()*/ UINT_MAX, cdfMax = 0; unsigned int minValue = /*std::numeric_limits::max()*/ UINT_MAX, maxValue = 0; @@ -211,8 +207,7 @@ void vp::equalizeHistogram(vpImage &I) // Construct the look-up table unsigned char lut[256]; for (unsigned int x = minValue; x <= maxValue; x++) { - lut[x] = vpMath::round((cdf[x] - cdfMin) / (double)(nbPixels - cdfMin) * - 255.0); + lut[x] = vpMath::round((cdf[x] - cdfMin) / (double)(nbPixels - cdfMin) * 255.0); } I.performLut(lut); @@ -228,8 +223,7 @@ void vp::equalizeHistogram(vpImage &I) \param I1 : The first grayscale image. \param I2 : The second grayscale image after histogram equalization. */ -void vp::equalizeHistogram(const vpImage &I1, - vpImage &I2) +void vp::equalizeHistogram(const vpImage &I1, vpImage &I2) { I2 = I1; vp::equalizeHistogram(I2); @@ -297,18 +291,15 @@ void vp::equalizeHistogram(vpImage &I, const bool useHSV) unsigned int size = I.getWidth() * I.getHeight(); // Convert from RGBa to HSV - vpImageConvert::RGBaToHSV((unsigned char *)I.bitmap, - (unsigned char *)hue.bitmap, - (unsigned char *)saturation.bitmap, - (unsigned char *)value.bitmap, size); + vpImageConvert::RGBaToHSV((unsigned char *)I.bitmap, (unsigned char *)hue.bitmap, + (unsigned char *)saturation.bitmap, (unsigned char *)value.bitmap, size); // Histogram equalization on the value plane vp::equalizeHistogram(value); // Convert from HSV to RGBa - vpImageConvert::HSVToRGBa( - (unsigned char *)hue.bitmap, (unsigned char *)saturation.bitmap, - (unsigned char *)value.bitmap, (unsigned char *)I.bitmap, size); + vpImageConvert::HSVToRGBa((unsigned char *)hue.bitmap, (unsigned char *)saturation.bitmap, + (unsigned char *)value.bitmap, (unsigned char *)I.bitmap, size); } } @@ -326,8 +317,7 @@ void vp::equalizeHistogram(vpImage &I, const bool useHSV) value channel (in HSV space), otherwise the histogram equalization is performed independently on the RGB channels. */ -void vp::equalizeHistogram(const vpImage &I1, vpImage &I2, - const bool useHSV) +void vp::equalizeHistogram(const vpImage &I1, vpImage &I2, const bool useHSV) { I2 = I1; vp::equalizeHistogram(I2, useHSV); @@ -347,15 +337,13 @@ void vp::gammaCorrection(vpImage &I, const double gamma) if (gamma > 0) { inverse_gamma = 1.0 / gamma; } else { - throw vpException(vpException::badValue, - "The gamma value must be positive !"); + throw vpException(vpException::badValue, "The gamma value must be positive !"); } // Construct the look-up table unsigned char lut[256]; for (unsigned int i = 0; i < 256; i++) { - lut[i] = vpMath::saturate( - pow((double)i / 255.0, inverse_gamma) * 255.0); + lut[i] = vpMath::saturate(pow((double)i / 255.0, inverse_gamma) * 255.0); } I.performLut(lut); @@ -370,8 +358,7 @@ void vp::gammaCorrection(vpImage &I, const double gamma) \param I2 : The second grayscale image after gamma correction. \param gamma : Gamma value. */ -void vp::gammaCorrection(const vpImage &I1, - vpImage &I2, const double gamma) +void vp::gammaCorrection(const vpImage &I1, vpImage &I2, const double gamma) { I2 = I1; vp::gammaCorrection(I2, gamma); @@ -391,21 +378,16 @@ void vp::gammaCorrection(vpImage &I, const double gamma) if (gamma > 0) { inverse_gamma = 1.0 / gamma; } else { - throw vpException(vpException::badValue, - "The gamma value must be positive !"); + throw vpException(vpException::badValue, "The gamma value must be positive !"); } // Construct the look-up table vpRGBa lut[256]; for (unsigned int i = 0; i < 256; i++) { - lut[i].R = vpMath::saturate( - pow((double)i / 255.0, inverse_gamma) * 255.0); - lut[i].G = vpMath::saturate( - pow((double)i / 255.0, inverse_gamma) * 255.0); - lut[i].B = vpMath::saturate( - pow((double)i / 255.0, inverse_gamma) * 255.0); - lut[i].A = vpMath::saturate( - pow((double)i / 255.0, inverse_gamma) * 255.0); + lut[i].R = vpMath::saturate(pow((double)i / 255.0, inverse_gamma) * 255.0); + lut[i].G = vpMath::saturate(pow((double)i / 255.0, inverse_gamma) * 255.0); + lut[i].B = vpMath::saturate(pow((double)i / 255.0, inverse_gamma) * 255.0); + lut[i].A = vpMath::saturate(pow((double)i / 255.0, inverse_gamma) * 255.0); } I.performLut(lut); @@ -420,8 +402,7 @@ void vp::gammaCorrection(vpImage &I, const double gamma) \param I2 : The second color image after gamma correction. \param gamma : Gamma value. */ -void vp::gammaCorrection(const vpImage &I1, vpImage &I2, - const double gamma) +void vp::gammaCorrection(const vpImage &I1, vpImage &I2, const double gamma) { I2 = I1; vp::gammaCorrection(I2, gamma); @@ -463,8 +444,7 @@ void vp::stretchContrast(vpImage &I) \param I1 : The first input grayscale image. \param I2 : The second output grayscale image. */ -void vp::stretchContrast(const vpImage &I1, - vpImage &I2) +void vp::stretchContrast(const vpImage &I1, vpImage &I2) { // Copy I1 to I2 I2 = I1; @@ -577,11 +557,10 @@ void vp::stretchContrastHSV(vpImage &I) unsigned int size = I.getWidth() * I.getHeight(); // Convert RGB to HSV - vpImage hueImage(I.getHeight(), I.getWidth()), - saturationImage(I.getHeight(), I.getWidth()), + vpImage hueImage(I.getHeight(), I.getWidth()), saturationImage(I.getHeight(), I.getWidth()), valueImage(I.getHeight(), I.getWidth()); - vpImageConvert::RGBaToHSV((unsigned char *)I.bitmap, hueImage.bitmap, - saturationImage.bitmap, valueImage.bitmap, size); + vpImageConvert::RGBaToHSV((unsigned char *)I.bitmap, hueImage.bitmap, saturationImage.bitmap, valueImage.bitmap, + size); // Find min and max Saturation and Value double minSaturation, maxSaturation, minValue, maxValue; @@ -595,8 +574,7 @@ void vp::stretchContrastHSV(vpImage &I) // Stretch Saturation if (maxSaturation - minSaturation > 0.0) { while (ptrCurrent != ptrEnd) { - *ptrCurrent = - (*ptrCurrent - minSaturation) / (maxSaturation - minSaturation); + *ptrCurrent = (*ptrCurrent - minSaturation) / (maxSaturation - minSaturation); ++ptrCurrent; } } @@ -614,8 +592,7 @@ void vp::stretchContrastHSV(vpImage &I) } // Convert HSV to RGBa - vpImageConvert::HSVToRGBa(hueImage.bitmap, saturationImage.bitmap, - valueImage.bitmap, (unsigned char *)I.bitmap, + vpImageConvert::HSVToRGBa(hueImage.bitmap, saturationImage.bitmap, valueImage.bitmap, (unsigned char *)I.bitmap, size); } @@ -644,8 +621,7 @@ void vp::stretchContrastHSV(const vpImage &I1, vpImage &I2) \param size : Size (must be odd) of the Gaussian blur kernel. \param weight : Weight (between [0 - 1[) for the sharpening process. */ -void vp::unsharpMask(vpImage &I, const unsigned int size, - const double weight) +void vp::unsharpMask(vpImage &I, const unsigned int size, const double weight) { if (weight < 1.0 && weight >= 0.0) { // Gaussian blurred image @@ -654,10 +630,8 @@ void vp::unsharpMask(vpImage &I, const unsigned int size, // Unsharp mask for (unsigned int cpt = 0; cpt < I.getSize(); cpt++) { - double val = - (I.bitmap[cpt] - weight * I_blurred.bitmap[cpt]) / (1 - weight); - I.bitmap[cpt] = vpMath::saturate( - val); // val > 255 ? 255 : (val < 0 ? 0 : val); + double val = (I.bitmap[cpt] - weight * I_blurred.bitmap[cpt]) / (1 - weight); + I.bitmap[cpt] = vpMath::saturate(val); // val > 255 ? 255 : (val < 0 ? 0 : val); } } } @@ -672,8 +646,7 @@ void vp::unsharpMask(vpImage &I, const unsigned int size, \param size : Size (must be odd) of the Gaussian blur kernel. \param weight : Weight (between [0 - 1[) for the sharpening process. */ -void vp::unsharpMask(const vpImage &I1, - vpImage &I2, const unsigned int size, +void vp::unsharpMask(const vpImage &I1, vpImage &I2, const unsigned int size, const double weight) { // Copy I1 to I2 @@ -690,8 +663,7 @@ void vp::unsharpMask(const vpImage &I1, \param size : Size (must be odd) of the Gaussian blur kernel. \param weight : Weight (between [0 - 1[) for the sharpening process. */ -void vp::unsharpMask(vpImage &I, const unsigned int size, - const double weight) +void vp::unsharpMask(vpImage &I, const unsigned int size, const double weight) { if (weight < 1.0 && weight >= 0.0) { // Gaussian blurred image @@ -705,12 +677,9 @@ void vp::unsharpMask(vpImage &I, const unsigned int size, // Unsharp mask for (unsigned int cpt = 0; cpt < I.getSize(); cpt++) { - double val_R = - (I.bitmap[cpt].R - weight * I_blurred_R.bitmap[cpt]) / (1 - weight); - double val_G = - (I.bitmap[cpt].G - weight * I_blurred_G.bitmap[cpt]) / (1 - weight); - double val_B = - (I.bitmap[cpt].B - weight * I_blurred_B.bitmap[cpt]) / (1 - weight); + double val_R = (I.bitmap[cpt].R - weight * I_blurred_R.bitmap[cpt]) / (1 - weight); + double val_G = (I.bitmap[cpt].G - weight * I_blurred_G.bitmap[cpt]) / (1 - weight); + double val_B = (I.bitmap[cpt].B - weight * I_blurred_B.bitmap[cpt]) / (1 - weight); I.bitmap[cpt].R = vpMath::saturate(val_R); I.bitmap[cpt].G = vpMath::saturate(val_G); @@ -729,8 +698,7 @@ void vp::unsharpMask(vpImage &I, const unsigned int size, \param size : Size (must be odd) of the Gaussian blur kernel. \param weight : Weight (between [0 - 1[) for the sharpening process. */ -void vp::unsharpMask(const vpImage &I1, vpImage &I2, - const unsigned int size, const double weight) +void vp::unsharpMask(const vpImage &I1, vpImage &I2, const unsigned int size, const double weight) { // Copy I1 to I2 I2 = I1; diff --git a/modules/imgproc/src/vpMorph.cpp b/modules/imgproc/src/vpMorph.cpp index e82bc50984..9653a2c628 100644 --- a/modules/imgproc/src/vpMorph.cpp +++ b/modules/imgproc/src/vpMorph.cpp @@ -102,12 +102,10 @@ void vp::fillHoles(vpImage &I } #else // Create flood fill mask - vpImage flood_fill_mask(I.getHeight() + 2, I.getWidth() + 2, - 0); + vpImage flood_fill_mask(I.getHeight() + 2, I.getWidth() + 2, 0); // Copy I to mask + add border padding for (unsigned int i = 0; i < I.getHeight(); i++) { - memcpy(flood_fill_mask[i + 1] + 1, I[i], - sizeof(unsigned char) * I.getWidth()); + memcpy(flood_fill_mask[i + 1] + 1, I[i], sizeof(unsigned char) * I.getWidth()); } // Perform flood fill @@ -116,8 +114,7 @@ void vp::fillHoles(vpImage &I // Get current mask vpImage mask(I.getHeight(), I.getWidth()); for (unsigned int i = 0; i < mask.getHeight(); i++) { - memcpy(mask[i], flood_fill_mask[i + 1] + 1, - sizeof(unsigned char) * mask.getWidth()); + memcpy(mask[i], flood_fill_mask[i + 1] + 1, sizeof(unsigned char) * mask.getWidth()); } // Get image with holes filled @@ -145,13 +142,10 @@ void vp::fillHoles(vpImage &I \param h_kp1 : Image morphologically reconstructed. \param connexity : Type of connexity. */ -void vp::reconstruct(const vpImage &marker, - const vpImage &mask, - vpImage &h_kp1 /*alias I */, - const vpImageMorphology::vpConnexityType &connexity) +void vp::reconstruct(const vpImage &marker, const vpImage &mask, + vpImage &h_kp1 /*alias I */, const vpImageMorphology::vpConnexityType &connexity) { - if (marker.getHeight() != mask.getHeight() || - marker.getWidth() != mask.getWidth()) { + if (marker.getHeight() != mask.getHeight() || marker.getWidth() != mask.getWidth()) { std::cerr << "marker.getHeight() != mask.getHeight() || " "marker.getWidth() != mask.getWidth()" << std::endl; diff --git a/modules/imgproc/src/vpRetinex.cpp b/modules/imgproc/src/vpRetinex.cpp index 307016131c..ccb8d91441 100644 --- a/modules/imgproc/src/vpRetinex.cpp +++ b/modules/imgproc/src/vpRetinex.cpp @@ -95,9 +95,7 @@ #define MAX_RETINEX_SCALES 8 -std::vector retinexScalesDistribution(const int scaleDiv, - const int level, - const int scale) +std::vector retinexScalesDistribution(const int scaleDiv, const int level, const int scale) { std::vector scales(MAX_RETINEX_SCALES); @@ -120,16 +118,14 @@ std::vector retinexScalesDistribution(const int scaleDiv, case vp::RETINEX_LOW: size_step = std::log(scale - 2.0) / (double)scaleDiv; for (i = 0; i < scaleDiv; i++) { - scales[(size_t)i] = - 2.0 + std::pow(10.0, (i * size_step) / std::log(10.0)); + scales[(size_t)i] = 2.0 + std::pow(10.0, (i * size_step) / std::log(10.0)); } break; case vp::RETINEX_HIGH: size_step = std::log(scale - 2.0) / (double)scaleDiv; for (i = 0; i < scaleDiv; i++) { - scales[(size_t)i] = - scale - std::pow(10.0, (i * size_step) / std::log(10.0)); + scales[(size_t)i] = scale - std::pow(10.0, (i * size_step) / std::log(10.0)); } break; @@ -143,13 +139,12 @@ std::vector retinexScalesDistribution(const int scaleDiv, // See: http://imagej.net/Retinex and // https://docs.gimp.org/en/plug-in-retinex.html -void MSRCR(vpImage &I, const int _scale, const int scaleDiv, - const int level, const double dynamic, const int _kernelSize) +void MSRCR(vpImage &I, const int _scale, const int scaleDiv, const int level, const double dynamic, + const int _kernelSize) { // Calculate the scales of filtering according to the number of filter and // their distribution. - std::vector retinexScales = - retinexScalesDistribution(scaleDiv, level, _scale); + std::vector retinexScales = retinexScalesDistribution(scaleDiv, level, _scale); // Filtering according to the various scales. // Summarize the results of the various filters according to a specific @@ -169,8 +164,7 @@ void MSRCR(vpImage &I, const int _scale, const int scaleDiv, for (int channel = 0; channel < 3; channel++) { doubleRGB[(size_t)channel] = vpImage(I.getHeight(), I.getWidth()); - doubleResRGB[(size_t)channel] = - vpImage(I.getHeight(), I.getWidth()); + doubleResRGB[(size_t)channel] = vpImage(I.getHeight(), I.getWidth()); for (unsigned int cpt = 0; cpt < size; cpt++) { // Shift the pixel values by 1 to avoid problem with log(0) @@ -195,16 +189,14 @@ void MSRCR(vpImage &I, const int _scale, const int scaleDiv, for (int sc = 0; sc < scaleDiv; sc++) { vpImage blurImage; double sigma = retinexScales[(size_t)sc]; - vpImageFilter::gaussianBlur(doubleRGB[(size_t)channel], blurImage, - (unsigned int)kernelSize, sigma); + vpImageFilter::gaussianBlur(doubleRGB[(size_t)channel], blurImage, (unsigned int)kernelSize, sigma); for (unsigned int cpt = 0; cpt < size; cpt++) { // Summarize the filtered values. // In fact one calculates a ratio between the original values and the // filtered values. doubleResRGB[(size_t)channel].bitmap[cpt] += - weight * (std::log(doubleRGB[(size_t)channel].bitmap[cpt]) - - std::log(blurImage.bitmap[cpt])); + weight * (std::log(doubleRGB[(size_t)channel].bitmap[cpt]) - std::log(blurImage.bitmap[cpt])); } } } @@ -213,31 +205,21 @@ void MSRCR(vpImage &I, const int _scale, const int scaleDiv, const double gain = 1.0, alpha = 128.0, offset = 0.0; for (unsigned int cpt = 0; cpt < size; cpt++) { - double logl = std::log( - (double)(I.bitmap[cpt].R + I.bitmap[cpt].G + I.bitmap[cpt].B + 3.0)); + double logl = std::log((double)(I.bitmap[cpt].R + I.bitmap[cpt].G + I.bitmap[cpt].B + 3.0)); - dest[cpt * 3] = gain * - (std::log(alpha * doubleRGB[0].bitmap[cpt]) - logl) * - doubleResRGB[0].bitmap[cpt] + - offset; + dest[cpt * 3] = gain * (std::log(alpha * doubleRGB[0].bitmap[cpt]) - logl) * doubleResRGB[0].bitmap[cpt] + offset; dest[cpt * 3 + 1] = - gain * (std::log(alpha * doubleRGB[1].bitmap[cpt]) - logl) * - doubleResRGB[1].bitmap[cpt] + - offset; + gain * (std::log(alpha * doubleRGB[1].bitmap[cpt]) - logl) * doubleResRGB[1].bitmap[cpt] + offset; dest[cpt * 3 + 2] = - gain * (std::log(alpha * doubleRGB[2].bitmap[cpt]) - logl) * - doubleResRGB[2].bitmap[cpt] + - offset; + gain * (std::log(alpha * doubleRGB[2].bitmap[cpt]) - logl) * doubleResRGB[2].bitmap[cpt] + offset; } double sum = std::accumulate(dest.begin(), dest.end(), 0.0); double mean = sum / dest.size(); std::vector diff(dest.size()); - std::transform(dest.begin(), dest.end(), diff.begin(), - std::bind2nd(std::minus(), mean)); - double sq_sum = - std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); + std::transform(dest.begin(), dest.end(), diff.begin(), std::bind2nd(std::minus(), mean)); + double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); double stdev = std::sqrt(sq_sum / dest.size()); double mini = mean - dynamic * stdev; @@ -249,12 +231,9 @@ void MSRCR(vpImage &I, const int _scale, const int scaleDiv, } for (unsigned int cpt = 0; cpt < size; cpt++) { - I.bitmap[cpt].R = vpMath::saturate( - (255.0 * (dest[cpt * 3 + 0] - mini) / range)); - I.bitmap[cpt].G = vpMath::saturate( - (255.0 * (dest[cpt * 3 + 1] - mini) / range)); - I.bitmap[cpt].B = vpMath::saturate( - (255.0 * (dest[cpt * 3 + 2] - mini) / range)); + I.bitmap[cpt].R = vpMath::saturate((255.0 * (dest[cpt * 3 + 0] - mini) / range)); + I.bitmap[cpt].G = vpMath::saturate((255.0 * (dest[cpt * 3 + 1] - mini) / range)); + I.bitmap[cpt].B = vpMath::saturate((255.0 * (dest[cpt * 3 + 2] - mini) / range)); } } @@ -276,8 +255,8 @@ void MSRCR(vpImage &I, const int _scale, const int scaleDiv, saturated images. \param kernelSize : Kernel size for the gaussian blur operation. If -1, the kernel size is calculated from the image size. */ -void vp::retinex(vpImage &I, const int scale, const int scaleDiv, - const int level, const double dynamic, const int kernelSize) +void vp::retinex(vpImage &I, const int scale, const int scaleDiv, const int level, const double dynamic, + const int kernelSize) { // Assert scale if (scale < 16 || scale > 250) { @@ -287,8 +266,7 @@ void vp::retinex(vpImage &I, const int scale, const int scaleDiv, // Assert scaleDiv if (scaleDiv < 1 || scaleDiv > 8) { - std::cerr << "Scale division must be between the interval [1 - 8]" - << std::endl; + std::cerr << "Scale division must be between the interval [1 - 8]" << std::endl; return; } @@ -319,8 +297,7 @@ void vp::retinex(vpImage &I, const int scale, const int scaleDiv, saturated images. \param kernelSize : Kernel size for the gaussian blur operation. If -1, the kernel size is calculated from the image size. */ -void vp::retinex(const vpImage &I1, vpImage &I2, - const int scale, const int scaleDiv, const int level, +void vp::retinex(const vpImage &I1, vpImage &I2, const int scale, const int scaleDiv, const int level, const double dynamic, const int kernelSize) { I2 = I1; diff --git a/modules/imgproc/src/vpThreshold.cpp b/modules/imgproc/src/vpThreshold.cpp index 18a2530dbf..189e9aa402 100644 --- a/modules/imgproc/src/vpThreshold.cpp +++ b/modules/imgproc/src/vpThreshold.cpp @@ -52,8 +52,7 @@ bool isBimodal(const std::vector &hist_float) int modes = 0; for (size_t cpt = 1; cpt < hist_float.size() - 1; cpt++) { - if (hist_float[cpt - 1] < hist_float[cpt] && - hist_float[cpt] > hist_float[cpt + 1]) { + if (hist_float[cpt - 1] < hist_float[cpt] && hist_float[cpt] > hist_float[cpt + 1]) { modes++; } @@ -77,14 +76,11 @@ int computeThresholdHuang(const vpHistogram &hist) // Find first and last non-empty bin size_t first, last; - for (first = 0; - first < (size_t)hist.getSize() && hist[(unsigned char)first] == 0; - first++) { + for (first = 0; first < (size_t)hist.getSize() && hist[(unsigned char)first] == 0; first++) { // do nothing } - for (last = (size_t)hist.getSize() - 1; - last > first && hist[(unsigned char)last] == 0; last--) { + for (last = (size_t)hist.getSize() - 1; last > first && hist[(unsigned char)last] == 0; last--) { // do nothing } @@ -167,22 +163,19 @@ int computeThresholdIntermodes(const vpHistogram &hist) while (!isBimodal(hist_float)) { // Smooth with a 3 point running mean filter for (size_t cpt = 1; cpt < hist_float.size() - 1; cpt++) { - hist_float[cpt] = - (hist_float[cpt - 1] + hist_float[cpt] + hist_float[cpt + 1]) / 3; + hist_float[cpt] = (hist_float[cpt - 1] + hist_float[cpt] + hist_float[cpt + 1]) / 3; } // First value hist_float[0] = (hist_float[0] + hist_float[1]) / 2.0f; // Last value - hist_float[hist_float.size() - 1] = - (hist_float.size() - 2 + hist_float.size() - 1) / 2.0f; + hist_float[hist_float.size() - 1] = (hist_float.size() - 2 + hist_float.size() - 1) / 2.0f; iter++; if (iter > 10000) { - std::cerr << "Intermodes Threshold not found after 10000 iterations!" - << std::endl; + std::cerr << "Intermodes Threshold not found after 10000 iterations!" << std::endl; return -1; } } @@ -190,8 +183,7 @@ int computeThresholdIntermodes(const vpHistogram &hist) // The threshold is the mean between the two peaks. int tt = 0; for (size_t cpt = 1; cpt < hist_float.size() - 1; cpt++) { - if (hist_float[cpt - 1] < hist_float[cpt] && - hist_float[cpt] > hist_float[cpt + 1]) { + if (hist_float[cpt - 1] < hist_float[cpt] && hist_float[cpt] > hist_float[cpt + 1]) { // Mode tt += (int)cpt; } @@ -200,8 +192,7 @@ int computeThresholdIntermodes(const vpHistogram &hist) return (int)std::floor(tt / 2.0); // vpMath::round(tt / 2.0); } -int computeThresholdIsoData(const vpHistogram &hist, - const unsigned int imageSize) +int computeThresholdIsoData(const vpHistogram &hist, const unsigned int imageSize) { int threshold = 0; @@ -219,16 +210,14 @@ int computeThresholdIsoData(const vpHistogram &hist, // STEP 2: compute Mean above T (MAT) and Mean below T (MBT) using T from float MBT = sum_ip[(size_t)(T - 2)] / cumsum[(size_t)(T - 2)]; - float MAT = (sum_ip.back() - sum_ip[(size_t)(T - 1)]) / - (cumsum.back() - cumsum[(size_t)(T - 1)]); + float MAT = (sum_ip.back() - sum_ip[(size_t)(T - 1)]) / (cumsum.back() - cumsum[(size_t)(T - 1)]); int T2 = vpMath::round((MAT + MBT) / 2.0f); //% STEP 3 to n: repeat step 2 if T(i)~=T(i-1) while (std::abs(T2 - T) >= 1) { MBT = sum_ip[(size_t)(T2 - 2)] / cumsum[(size_t)(T2 - 2)]; - MAT = (sum_ip.back() - sum_ip[(size_t)(T2 - 1)]) / - (cumsum.back() - cumsum[(size_t)(T2 - 1)]); + MAT = (sum_ip.back() - sum_ip[(size_t)(T2 - 1)]) / (cumsum.back() - cumsum[(size_t)(T2 - 1)]); T = T2; T2 = vpMath::round((MAT + MBT) / 2.0f); @@ -238,8 +227,7 @@ int computeThresholdIsoData(const vpHistogram &hist, return threshold; } -int computeThresholdMean(const vpHistogram &hist, - const unsigned int imageSize) +int computeThresholdMean(const vpHistogram &hist, const unsigned int imageSize) { // C. A. Glasbey, "An analysis of histogram-based thresholding algorithms," // CVGIP: Graphical Models and Image Processing, vol. 55, pp. 532-537, 1993. @@ -252,8 +240,7 @@ int computeThresholdMean(const vpHistogram &hist, return (int)std::floor(sum_ip / imageSize); } -int computeThresholdOtsu(const vpHistogram &hist, - const unsigned int imageSize) +int computeThresholdOtsu(const vpHistogram &hist, const unsigned int imageSize) { // Otsu, N (1979), "A threshold selection method from gray-level // histograms", IEEE Trans. Sys., Man., Cyber. 9: 62-66, @@ -326,8 +313,7 @@ int computeThresholdTriangle(vpHistogram &hist) // First / last index when hist(cpt) == 0 left_bound = left_bound > 0 ? left_bound - 1 : left_bound; - right_bound = - right_bound < (int)hist.getSize() - 1 ? right_bound + 1 : right_bound; + right_bound = right_bound < (int)hist.getSize() - 1 ? right_bound + 1 : right_bound; // Use the largest bound bool flip = false; @@ -385,10 +371,8 @@ int computeThresholdTriangle(vpHistogram &hist) \param backgroundValue : Value to set to the background. \param foregroundValue : Value to set to the foreground. */ -unsigned char vp::autoThreshold(vpImage &I, - const vpAutoThresholdMethod &method, - const unsigned char backgroundValue, - const unsigned char foregroundValue) +unsigned char vp::autoThreshold(vpImage &I, const vpAutoThresholdMethod &method, + const unsigned char backgroundValue, const unsigned char foregroundValue) { if (I.getSize() == 0) { return 0; @@ -429,8 +413,8 @@ unsigned char vp::autoThreshold(vpImage &I, if (threshold != -1) { // Threshold - vpImageTools::binarise(I, (unsigned char)threshold, (unsigned char)255, - backgroundValue, foregroundValue, foregroundValue); + vpImageTools::binarise(I, (unsigned char)threshold, (unsigned char)255, backgroundValue, foregroundValue, + foregroundValue); } return threshold; diff --git a/modules/imgproc/test/testAutoThreshold.cpp b/modules/imgproc/test/testAutoThreshold.cpp index a2257df0bd..7307dc02f1 100644 --- a/modules/imgproc/test/testAutoThreshold.cpp +++ b/modules/imgproc/test/testAutoThreshold.cpp @@ -51,10 +51,8 @@ // 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); +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. @@ -65,8 +63,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, \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) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user) { fprintf(stdout, "\n\ Test automatic thresholding.\n\ @@ -110,8 +107,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user) { const char *optarg_; int c; @@ -203,8 +199,7 @@ int main(int argc, const char **argv) 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; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; exit(EXIT_FAILURE); } } @@ -215,8 +210,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -225,13 +219,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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(EXIT_FAILURE); } @@ -242,8 +233,7 @@ int main(int argc, const char **argv) filename = vpIoTools::createFilePath(ipath, "calibration/grid36-03.pgm"); vpImage I; vpImageIo::read(I, filename); - std::cout << "Read: " << filename << " (" << I.getWidth() << "x" - << I.getHeight() << ")" << std::endl; + std::cout << "Read: " << filename << " (" << I.getWidth() << "x" << I.getHeight() << ")" << std::endl; vpImage I_thresh = I; @@ -251,11 +241,9 @@ int main(int argc, const char **argv) double t = vpTime::measureTimeMs(); double threshold = vp::autoThreshold(I_thresh, vp::AUTO_THRESHOLD_HUANG); t = vpTime::measureTimeMs() - t; - std::cout << "\nAutomatic thresholding (Huang): " << threshold - << " ; t=" << t << " ms" << std::endl; + std::cout << "\nAutomatic thresholding (Huang): " << threshold << " ; t=" << t << " ms" << std::endl; - filename = - vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_huang.pgm"); + filename = vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_huang.pgm"); vpImageIo::write(I_thresh, filename); std::cout << "Write: " << filename << std::endl; @@ -264,11 +252,9 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); threshold = vp::autoThreshold(I_thresh, vp::AUTO_THRESHOLD_INTERMODES); t = vpTime::measureTimeMs() - t; - std::cout << "\nAutomatic thresholding (Intermodes): " << threshold - << " ; t=" << t << " ms" << std::endl; + std::cout << "\nAutomatic thresholding (Intermodes): " << threshold << " ; t=" << t << " ms" << std::endl; - filename = vpIoTools::createFilePath( - opath, "grid36-03_auto_thresh_intermodes.pgm"); + filename = vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_intermodes.pgm"); vpImageIo::write(I_thresh, filename); std::cout << "Write: " << filename << std::endl; @@ -277,11 +263,9 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); threshold = vp::autoThreshold(I_thresh, vp::AUTO_THRESHOLD_ISODATA); t = vpTime::measureTimeMs() - t; - std::cout << "\nAutomatic thresholding (IsoData): " << threshold - << " ; t=" << t << " ms" << std::endl; + std::cout << "\nAutomatic thresholding (IsoData): " << threshold << " ; t=" << t << " ms" << std::endl; - filename = - vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_isodata.pgm"); + filename = vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_isodata.pgm"); vpImageIo::write(I_thresh, filename); std::cout << "Write: " << filename << std::endl; @@ -290,11 +274,9 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); threshold = vp::autoThreshold(I_thresh, vp::AUTO_THRESHOLD_MEAN); t = vpTime::measureTimeMs() - t; - std::cout << "\nAutomatic thresholding (Mean): " << threshold - << " ; t=" << t << " ms" << std::endl; + std::cout << "\nAutomatic thresholding (Mean): " << threshold << " ; t=" << t << " ms" << std::endl; - filename = - vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_mean.pgm"); + filename = vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_mean.pgm"); vpImageIo::write(I_thresh, filename); std::cout << "Write: " << filename << std::endl; @@ -303,11 +285,9 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); threshold = vp::autoThreshold(I_thresh, vp::AUTO_THRESHOLD_OTSU); t = vpTime::measureTimeMs() - t; - std::cout << "\nAutomatic thresholding (Otsu): " << threshold - << " ; t=" << t << " ms" << std::endl; + std::cout << "\nAutomatic thresholding (Otsu): " << threshold << " ; t=" << t << " ms" << std::endl; - filename = - vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_otsu.pgm"); + filename = vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_otsu.pgm"); vpImageIo::write(I_thresh, filename); std::cout << "Write: " << filename << std::endl; @@ -316,11 +296,9 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); threshold = vp::autoThreshold(I_thresh, vp::AUTO_THRESHOLD_TRIANGLE); t = vpTime::measureTimeMs() - t; - std::cout << "\nAutomatic thresholding (Triangle): " << threshold - << " ; t=" << t << " ms" << std::endl; + std::cout << "\nAutomatic thresholding (Triangle): " << threshold << " ; t=" << t << " ms" << std::endl; - filename = vpIoTools::createFilePath( - opath, "grid36-03_auto_thresh_Triangle.pgm"); + filename = vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_Triangle.pgm"); vpImageIo::write(I_thresh, filename); std::cout << "Write: " << filename << std::endl; diff --git a/modules/imgproc/test/testConnectedComponents.cpp b/modules/imgproc/test/testConnectedComponents.cpp index 7832cc15d7..d5fb6ede82 100644 --- a/modules/imgproc/test/testConnectedComponents.cpp +++ b/modules/imgproc/test/testConnectedComponents.cpp @@ -52,10 +52,8 @@ // 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); +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); bool checkLabels(const vpImage &label1, const vpImage &label2); /* @@ -67,8 +65,7 @@ bool checkLabels(const vpImage &label1, const vpImage &label2); \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) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user) { fprintf(stdout, "\n\ Test connected components.\n\ @@ -112,8 +109,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user) { const char *optarg_; int c; @@ -155,15 +151,13 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool checkLabels(const vpImage &label1, const vpImage &label2) { - if (label1.getHeight() != label2.getHeight() || - label1.getWidth() != label2.getWidth()) + if (label1.getHeight() != label2.getHeight() || label1.getWidth() != label2.getWidth()) return false; std::map > map_label1, map_label2; for (unsigned int i = 0; i < label1.getHeight(); i++) { for (unsigned int j = 0; j < label1.getWidth(); j++) { - if ((label1[i][j] > 0 && label2[i][j] == 0) || - (label1[i][j] == 0 && label2[i][j] > 0)) { + if ((label1[i][j] > 0 && label2[i][j] == 0) || (label1[i][j] == 0 && label2[i][j] > 0)) { std::cerr << "label1[i][j] > 0 && label2[i][j] == 0 || label1[i][j] " "== 0 && label2[i][j] > 0" << std::endl; @@ -183,12 +177,10 @@ bool checkLabels(const vpImage &label1, const vpImage &label2) return false; } - for (std::map >::const_iterator it1 = - map_label1.begin(); - it1 != map_label1.end(); ++it1) { + for (std::map >::const_iterator it1 = map_label1.begin(); it1 != map_label1.end(); + ++it1) { // Get corresponding label in the other method - unsigned int i = (unsigned int)it1->second.front().get_i(), - j = (unsigned int)it1->second.front().get_j(); + unsigned int i = (unsigned int)it1->second.front().get_i(), j = (unsigned int)it1->second.front().get_j(); int lab2 = label2[i][j]; std::vector::const_iterator it_pt1 = it1->second.begin(); @@ -257,8 +249,7 @@ int main(int argc, const char **argv) 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; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; exit(EXIT_FAILURE); } } @@ -269,8 +260,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -279,13 +269,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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(EXIT_FAILURE); } @@ -298,17 +285,14 @@ int main(int argc, const char **argv) vpImage I; std::cout << "Read image: " << filename << std::endl; vpImageIo::read(I, filename); - vpImageTools::binarise(I, (unsigned char)127, (unsigned char)255, - (unsigned char)0, (unsigned char)255, + vpImageTools::binarise(I, (unsigned char)127, (unsigned char)255, (unsigned char)0, (unsigned char)255, (unsigned char)255); - std::cout << "Image: " << I.getWidth() << "x" << I.getHeight() - << std::endl; + std::cout << "Image: " << I.getWidth() << "x" << I.getHeight() << std::endl; vpImage labels_connex4; int nbComponents = 0; double t = vpTime::measureTimeMs(); - vp::connectedComponents(I, labels_connex4, nbComponents, - vpImageMorphology::CONNEXITY_4); + vp::connectedComponents(I, labels_connex4, nbComponents, vpImageMorphology::CONNEXITY_4); t = vpTime::measureTimeMs() - t; std::cout << "\n4-connexity connected components:" << std::endl; std::cout << "Time: " << t << " ms" << std::endl; @@ -316,48 +300,39 @@ int main(int argc, const char **argv) vpImage labels_connex8; t = vpTime::measureTimeMs(); - vp::connectedComponents(I, labels_connex8, nbComponents, - vpImageMorphology::CONNEXITY_8); + vp::connectedComponents(I, labels_connex8, nbComponents, vpImageMorphology::CONNEXITY_8); t = vpTime::measureTimeMs() - t; std::cout << "\n8-connexity connected components:" << std::endl; std::cout << "Time: " << t << " ms" << std::endl; std::cout << "nbComponents=" << nbComponents << std::endl; // Save results - vpImage labels_connex4_color(labels_connex4.getHeight(), - labels_connex4.getWidth(), - vpRGBa(0, 0, 0, 0)); + vpImage labels_connex4_color(labels_connex4.getHeight(), labels_connex4.getWidth(), vpRGBa(0, 0, 0, 0)); for (unsigned int i = 0; i < labels_connex4.getHeight(); i++) { for (unsigned int j = 0; j < labels_connex4.getWidth(); j++) { if (labels_connex4[i][j] != 0) { - labels_connex4_color[i][j] = - vpRGBa(vpColor::getColor((unsigned int)labels_connex4[i][j]).R, - vpColor::getColor((unsigned int)labels_connex4[i][j]).G, - vpColor::getColor((unsigned int)labels_connex4[i][j]).B); + labels_connex4_color[i][j] = vpRGBa(vpColor::getColor((unsigned int)labels_connex4[i][j]).R, + vpColor::getColor((unsigned int)labels_connex4[i][j]).G, + vpColor::getColor((unsigned int)labels_connex4[i][j]).B); } } } - filename = - vpIoTools::createFilePath(opath, "Klimt_connected_components_4.ppm"); + filename = vpIoTools::createFilePath(opath, "Klimt_connected_components_4.ppm"); vpImageIo::write(labels_connex4_color, filename); - vpImage labels_connex8_color(labels_connex8.getHeight(), - labels_connex8.getWidth(), - vpRGBa(0, 0, 0, 0)); + vpImage labels_connex8_color(labels_connex8.getHeight(), labels_connex8.getWidth(), vpRGBa(0, 0, 0, 0)); for (unsigned int i = 0; i < labels_connex8.getHeight(); i++) { for (unsigned int j = 0; j < labels_connex8.getWidth(); j++) { if (labels_connex8[i][j] != 0) { - labels_connex8_color[i][j] = - vpRGBa(vpColor::getColor((unsigned int)labels_connex8[i][j]).R, - vpColor::getColor((unsigned int)labels_connex8[i][j]).G, - vpColor::getColor((unsigned int)labels_connex8[i][j]).B); + labels_connex8_color[i][j] = vpRGBa(vpColor::getColor((unsigned int)labels_connex8[i][j]).R, + vpColor::getColor((unsigned int)labels_connex8[i][j]).G, + vpColor::getColor((unsigned int)labels_connex8[i][j]).B); } } } - filename = - vpIoTools::createFilePath(opath, "Klimt_connected_components_8.ppm"); + filename = vpIoTools::createFilePath(opath, "Klimt_connected_components_8.ppm"); vpImageIo::write(labels_connex8_color, filename); #if (VISP_HAVE_OPENCV_VERSION >= 0x030000) @@ -370,8 +345,7 @@ int main(int argc, const char **argv) t_opencv = vpTime::measureTimeMs() - t_opencv; std::set set_labels_connex4_opencv; - vpImage labels_connex4_opencv((unsigned int)matLabels_4.rows, - (unsigned int)matLabels_4.cols); + vpImage labels_connex4_opencv((unsigned int)matLabels_4.rows, (unsigned int)matLabels_4.cols); for (int i = 0; i < matLabels_4.rows; i++) { for (int j = 0; j < matLabels_4.cols; j++) { labels_connex4_opencv[i][j] = matLabels_4.at(i, j); @@ -383,16 +357,13 @@ int main(int argc, const char **argv) std::cout << "\n4-connexity connected components (OpenCV):" << std::endl; std::cout << "Time: " << t_opencv << " ms" << std::endl; - std::cout << "nb components: " << set_labels_connex4_opencv.size() - << std::endl; + std::cout << "nb components: " << set_labels_connex4_opencv.size() << std::endl; bool check_label = checkLabels(labels_connex4_opencv, labels_connex4); - std::cout << "checkLabels(labels_connex4_opencv, labels_connex4): " - << check_label << std::endl; + std::cout << "checkLabels(labels_connex4_opencv, labels_connex4): " << check_label << std::endl; // std::cout << "(labels_connex4_opencv == labels_connex4)? " << // (labels_connex4_opencv == labels_connex4) << std::endl; if (!check_label) { - throw vpException(vpException::fatalError, - "(labels_connex4_opencv != labels_connex4)"); + throw vpException(vpException::fatalError, "(labels_connex4_opencv != labels_connex4)"); } cv::Mat matLabels_8; @@ -401,8 +372,7 @@ int main(int argc, const char **argv) t_opencv = vpTime::measureTimeMs() - t_opencv; std::set set_labels_connex8_opencv; - vpImage labels_connex8_opencv((unsigned int)matLabels_8.rows, - (unsigned int)matLabels_8.cols); + vpImage labels_connex8_opencv((unsigned int)matLabels_8.rows, (unsigned int)matLabels_8.cols); for (int i = 0; i < matLabels_8.rows; i++) { for (int j = 0; j < matLabels_8.cols; j++) { labels_connex8_opencv[i][j] = matLabels_8.at(i, j); @@ -413,18 +383,15 @@ int main(int argc, const char **argv) } std::cout << "\n8-connexity connected components (OpenCV):" << std::endl; - std::cout << "nb components: " << set_labels_connex8_opencv.size() - << std::endl; + std::cout << "nb components: " << set_labels_connex8_opencv.size() << std::endl; std::cout << "Time: " << t_opencv << " ms" << std::endl; check_label = checkLabels(labels_connex8_opencv, labels_connex8); - std::cout << "checkLabels(labels_connex8_opencv, labels_connex8): " - << check_label << std::endl; + std::cout << "checkLabels(labels_connex8_opencv, labels_connex8): " << check_label << std::endl; // std::cout << "(labels_connex8_opencv == labels_connex8)? " << // (labels_connex8_opencv == labels_connex8) << std::endl; if (!check_label) { - throw vpException(vpException::fatalError, - "(labels_connex8_opencv != labels_connex8)"); + throw vpException(vpException::fatalError, "(labels_connex8_opencv != labels_connex8)"); } #endif diff --git a/modules/imgproc/test/testContours.cpp b/modules/imgproc/test/testContours.cpp index 484531ecbe..9727deede3 100644 --- a/modules/imgproc/test/testContours.cpp +++ b/modules/imgproc/test/testContours.cpp @@ -53,10 +53,8 @@ // 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); +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. @@ -67,8 +65,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, \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) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user) { fprintf(stdout, "\n\ Test contours extraction.\n\ @@ -112,8 +109,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user) { const char *optarg_; int c; @@ -167,8 +163,7 @@ void printImage(const vpImage &I, const std::string &name) std::cout << std::setfill(' ') << std::setw(2) << i << " "; for (unsigned int j = 0; j < I.getWidth(); j++) { - std::cout << std::setfill(' ') << std::setw(2) - << static_cast(I[i][j]) << " "; + std::cout << std::setfill(' ') << std::setw(2) << static_cast(I[i][j]) << " "; } std::cout << std::endl; @@ -179,15 +174,12 @@ void displayContourInfo(const vp::vpContour &contour, const int level) { std::cout << "\nContour:" << std::endl; std::cout << "\tlevel: " << level << std::endl; - std::cout << "\tcontour type: " - << (contour.m_contourType == vp::CONTOUR_OUTER ? "outer contour" - : "hole contour") + std::cout << "\tcontour type: " << (contour.m_contourType == vp::CONTOUR_OUTER ? "outer contour" : "hole contour") << std::endl; std::cout << "\tnb children: " << contour.m_children.size() << std::endl; - for (std::vector::const_iterator it = - contour.m_children.begin(); - it != contour.m_children.end(); ++it) { + for (std::vector::const_iterator it = contour.m_children.begin(); it != contour.m_children.end(); + ++it) { displayContourInfo(**it, level + 1); } } @@ -244,8 +236,7 @@ int main(int argc, const char **argv) 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; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; exit(EXIT_FAILURE); } } @@ -256,8 +247,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -266,13 +256,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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(EXIT_FAILURE); } @@ -281,13 +268,10 @@ int main(int argc, const char **argv) // unsigned char image_data[14 * 10] = { - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, - 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, - 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, - 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, + 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; vpImage I_test_data(image_data, 14, 10, true); std::cout << "Test with image data:" << std::endl; @@ -300,24 +284,21 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs() - t; displayContourInfo(vp_contours, 0); - std::cout << "ViSP: nb contours=" << contours.size() << " ; t=" << t - << " ms" << std::endl; + std::cout << "ViSP: nb contours=" << contours.size() << " ; t=" << t << " ms" << std::endl; // Read Klimt.ppm filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm"); vpImage I; std::cout << "Read image: " << filename << std::endl; vpImageIo::read(I, filename); - vpImageTools::binarise(I, (unsigned char)127, (unsigned char)255, - (unsigned char)0, (unsigned char)1, + vpImageTools::binarise(I, (unsigned char)127, (unsigned char)255, (unsigned char)0, (unsigned char)1, (unsigned char)1); vpImage I2(I.getHeight(), I.getWidth()); for (unsigned int cpt = 0; cpt < I2.getSize(); cpt++) { I2.bitmap[cpt] = 255 * I.bitmap[cpt]; } - filename = - vpIoTools::createFilePath(opath, "Klimt_contours_binarise.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_contours_binarise.pgm"); vpImageIo::write(I2, filename); t = vpTime::measureTimeMs(); @@ -326,75 +307,63 @@ int main(int argc, const char **argv) displayContourInfo(vp_contours, 0); std::cout << "\nTest with Klimt image:" << std::endl; - std::cout << "ViSP: nb contours=" << contours.size() << " ; t=" << t - << " ms" << std::endl; + std::cout << "ViSP: nb contours=" << contours.size() << " ; t=" << t << " ms" << std::endl; // Draw and save vpImage I_draw_contours(I2.getHeight(), I2.getWidth(), 0); vp::drawContours(I_draw_contours, contours); - filename = - vpIoTools::createFilePath(opath, "Klimt_contours_extracted.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_contours_extracted.pgm"); vpImageIo::write(I_draw_contours, filename); - vpImage I_draw_contours_color(I2.getHeight(), I2.getWidth(), - vpRGBa(0, 0, 0)); + vpImage I_draw_contours_color(I2.getHeight(), I2.getWidth(), vpRGBa(0, 0, 0)); vp::drawContours(I_draw_contours_color, contours, vpColor::red); - filename = vpIoTools::createFilePath( - opath, "Klimt_contours_extracted_color.ppm"); + filename = vpIoTools::createFilePath(opath, "Klimt_contours_extracted_color.ppm"); vpImageIo::write(I_draw_contours_color, filename); // Test retrieve list vp::findContours(I, vp_contours, contours, vp::CONTOUR_RETR_LIST); - vpImage I_draw_contours_list(I2.getHeight(), I2.getWidth(), - 0); + vpImage I_draw_contours_list(I2.getHeight(), I2.getWidth(), 0); vpImage I_tmp_list(I.getHeight(), I.getWidth(), 0); vp::drawContours(I_tmp_list, contours); contours.clear(); - for (std::vector::const_iterator it = - vp_contours.m_children.begin(); + for (std::vector::const_iterator it = vp_contours.m_children.begin(); it != vp_contours.m_children.end(); ++it) { contours.push_back((*it)->m_points); } vp::drawContours(I_draw_contours_list, contours); - std::cout << "(I_tmp_list == I_draw_contours_list)? " - << (I_tmp_list == I_draw_contours_list) << std::endl; + std::cout << "(I_tmp_list == I_draw_contours_list)? " << (I_tmp_list == I_draw_contours_list) << std::endl; - filename = - vpIoTools::createFilePath(opath, "Klimt_contours_extracted_list.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_contours_extracted_list.pgm"); vpImageIo::write(I_draw_contours_list, filename); // Test retrieve external vp::findContours(I, vp_contours, contours, vp::CONTOUR_RETR_EXTERNAL); - vpImage I_draw_contours_external(I2.getHeight(), - I2.getWidth(), 0); + vpImage I_draw_contours_external(I2.getHeight(), I2.getWidth(), 0); vpImage I_tmp_external(I.getHeight(), I.getWidth(), 0); vp::drawContours(I_tmp_external, contours); contours.clear(); - for (std::vector::const_iterator it = - vp_contours.m_children.begin(); + for (std::vector::const_iterator it = vp_contours.m_children.begin(); it != vp_contours.m_children.end(); ++it) { contours.push_back((*it)->m_points); } vp::drawContours(I_draw_contours_external, contours); - std::cout << "(I_tmp_external == I_draw_contours_external)? " - << (I_tmp_external == I_draw_contours_external) << std::endl; + std::cout << "(I_tmp_external == I_draw_contours_external)? " << (I_tmp_external == I_draw_contours_external) + << std::endl; - filename = vpIoTools::createFilePath( - opath, "Klimt_contours_extracted_external.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_contours_extracted_external.pgm"); vpImageIo::write(I_draw_contours_external, filename); // Test fillHoles vpImage I_holes = I_draw_contours_external; - vpImageTools::binarise(I_holes, (unsigned char)127, (unsigned char)255, - (unsigned char)0, (unsigned char)255, + vpImageTools::binarise(I_holes, (unsigned char)127, (unsigned char)255, (unsigned char)0, (unsigned char)255, (unsigned char)255); t = vpTime::measureTimeMs(); @@ -402,8 +371,7 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs() - t; std::cout << "\nFill Holes: " << t << " ms" << std::endl; - filename = vpIoTools::createFilePath( - opath, "Klimt_contours_extracted_external_fill_holes.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_contours_extracted_external_fill_holes.pgm"); vpImageIo::write(I_holes, filename); #if VISP_HAVE_OPENCV_VERSION >= 0x030000 @@ -412,69 +380,55 @@ int main(int argc, const char **argv) std::vector > contours_opencv; double t_opencv = vpTime::measureTimeMs(); - cv::findContours(matImg, contours_opencv, cv::RETR_TREE, - cv::CHAIN_APPROX_NONE); + cv::findContours(matImg, contours_opencv, cv::RETR_TREE, cv::CHAIN_APPROX_NONE); t_opencv = vpTime::measureTimeMs() - t_opencv; - std::cout << "\nOpenCV: nb contours=" << contours_opencv.size() - << " ; t_opencv=" << t_opencv << " ms" << std::endl; + std::cout << "\nOpenCV: nb contours=" << contours_opencv.size() << " ; t_opencv=" << t_opencv << " ms" << std::endl; - vpImage I_draw_contours_opencv(I.getHeight(), I.getWidth(), - 0); - for (std::vector >::const_iterator it1 = - contours_opencv.begin(); + vpImage I_draw_contours_opencv(I.getHeight(), I.getWidth(), 0); + for (std::vector >::const_iterator it1 = contours_opencv.begin(); it1 != contours_opencv.end(); ++it1) { - for (std::vector::const_iterator it2 = it1->begin(); - it2 != it1->end(); ++it2) { + for (std::vector::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) { I_draw_contours_opencv[it2->y][it2->x] = 255; } } - std::cout << "(I_draw_contours_opencv == I_drawContours)? " - << (I_draw_contours_opencv == I_draw_contours) << std::endl; + std::cout << "(I_draw_contours_opencv == I_drawContours)? " << (I_draw_contours_opencv == I_draw_contours) + << std::endl; - filename = vpIoTools::createFilePath( - opath, "Klimt_contours_extracted_opencv.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_contours_extracted_opencv.pgm"); vpImageIo::write(I_draw_contours_opencv, filename); // Test retrieve list vpImageConvert::convert(I, matImg); contours_opencv.clear(); - cv::findContours(matImg, contours_opencv, cv::RETR_LIST, - cv::CHAIN_APPROX_NONE); + cv::findContours(matImg, contours_opencv, cv::RETR_LIST, cv::CHAIN_APPROX_NONE); I_draw_contours_opencv = 0; - for (std::vector >::const_iterator it1 = - contours_opencv.begin(); + for (std::vector >::const_iterator it1 = contours_opencv.begin(); it1 != contours_opencv.end(); ++it1) { - for (std::vector::const_iterator it2 = it1->begin(); - it2 != it1->end(); ++it2) { + for (std::vector::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) { I_draw_contours_opencv[it2->y][it2->x] = 255; } } std::cout << "(I_draw_contours_opencv == I_draw_contours_list)? " - << (I_draw_contours_opencv == I_draw_contours_list) - << std::endl; + << (I_draw_contours_opencv == I_draw_contours_list) << std::endl; // Test retrieve external vpImageConvert::convert(I, matImg); contours_opencv.clear(); - cv::findContours(matImg, contours_opencv, cv::RETR_EXTERNAL, - cv::CHAIN_APPROX_NONE); + cv::findContours(matImg, contours_opencv, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); I_draw_contours_opencv = 0; - for (std::vector >::const_iterator it1 = - contours_opencv.begin(); + for (std::vector >::const_iterator it1 = contours_opencv.begin(); it1 != contours_opencv.end(); ++it1) { - for (std::vector::const_iterator it2 = it1->begin(); - it2 != it1->end(); ++it2) { + for (std::vector::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) { I_draw_contours_opencv[it2->y][it2->x] = 255; } } std::cout << "(I_draw_contours_opencv == I_draw_contours_external)? " - << (I_draw_contours_opencv == I_draw_contours_external) - << std::endl; + << (I_draw_contours_opencv == I_draw_contours_external) << std::endl; #endif return EXIT_SUCCESS; diff --git a/modules/imgproc/test/testFloodFill.cpp b/modules/imgproc/test/testFloodFill.cpp index f72f4ab51f..1697b7cae7 100644 --- a/modules/imgproc/test/testFloodFill.cpp +++ b/modules/imgproc/test/testFloodFill.cpp @@ -53,10 +53,8 @@ // 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); +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. @@ -67,8 +65,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, \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) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user) { fprintf(stdout, "\n\ Test flood fill algorithm.\n\ @@ -111,8 +108,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user) { const char *optarg_; int c; @@ -172,8 +168,7 @@ void printImage(const vpImage &I, const std::string &name) std::cout << std::setfill(' ') << std::setw(2) << i << "|"; for (unsigned int j = 0; j < I.getWidth(); j++) { - std::cout << std::setfill(' ') << std::setw(2) - << static_cast(I[i][j]) << " "; + std::cout << std::setfill(' ') << std::setw(2) << static_cast(I[i][j]) << " "; } std::cout << std::endl; @@ -232,8 +227,7 @@ int main(int argc, const char **argv) 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; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; exit(EXIT_FAILURE); } } @@ -244,8 +238,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -254,13 +247,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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(EXIT_FAILURE); } @@ -268,67 +258,51 @@ int main(int argc, const char **argv) // Here starts really the test // - unsigned char image_data[8 * 8] = { - 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, - 1, 0, 1, 0, 0, 0, 1, 1, 1, 0, 1, 1, 1, 1, 0, 1, 1, 1, 1, 0, 0, 1, - 1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 1, 1, 0}; - vpImage I_test_flood_fill_4_connexity(image_data, 8, 8, - true); - vpImage I_test_flood_fill_8_connexity = - I_test_flood_fill_4_connexity; + unsigned char image_data[8 * 8] = {1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, + 1, 0, 1, 0, 0, 0, 1, 1, 1, 0, 1, 1, 1, 1, 0, 1, 1, 1, 1, 0, 0, 1, + 1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 1, 1, 0}; + vpImage I_test_flood_fill_4_connexity(image_data, 8, 8, true); + vpImage I_test_flood_fill_8_connexity = I_test_flood_fill_4_connexity; printImage(I_test_flood_fill_4_connexity, "Test image data"); unsigned char image_data_check_4_connexity[8 * 8] = { - 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, - 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 0, 1, 1, 1, 1, 0, 0, 1, - 1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 1, 1, 0}; - vpImage I_check_4_connexity(image_data_check_4_connexity, - 8, 8, true); + 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, + 1, 1, 1, 1, 0, 1, 1, 1, 1, 0, 0, 1, 1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 1, 1, 0}; + vpImage I_check_4_connexity(image_data_check_4_connexity, 8, 8, true); unsigned char image_data_check_8_connexity[8 * 8] = { - 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, - 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, - 1, 1, 1, 0, 1, 0, 0, 0, 1, 1, 1, 0, 1, 0, 0, 0, 1, 1, 1, 0}; - vpImage I_check_8_connexity(image_data_check_8_connexity, - 8, 8, true); + 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 0, 1, 0, 0, 0, 1, 1, 1, 0, 1, 0, 0, 0, 1, 1, 1, 0}; + vpImage I_check_8_connexity(image_data_check_8_connexity, 8, 8, true); // Test flood fill on test data 4-connexity - vp::floodFill(I_test_flood_fill_4_connexity, vpImagePoint(2, 2), 0, 1, - vpImageMorphology::CONNEXITY_4); - printImage(I_test_flood_fill_4_connexity, - "I_test_flood_fill_4_connexity"); + vp::floodFill(I_test_flood_fill_4_connexity, vpImagePoint(2, 2), 0, 1, vpImageMorphology::CONNEXITY_4); + printImage(I_test_flood_fill_4_connexity, "I_test_flood_fill_4_connexity"); if (I_test_flood_fill_4_connexity != I_check_4_connexity) { - throw vpException(vpException::fatalError, - "Problem with vp::floodFill() and 4-connexity!"); + throw vpException(vpException::fatalError, "Problem with vp::floodFill() and 4-connexity!"); } std::cout << "\n(I_test_flood_fill_4_connexity == I_check_4_connexity)? " - << (I_test_flood_fill_4_connexity == I_check_4_connexity) - << std::endl; + << (I_test_flood_fill_4_connexity == I_check_4_connexity) << std::endl; // Test flood fill on test data 8-connexity - vp::floodFill(I_test_flood_fill_8_connexity, vpImagePoint(2, 2), 0, 1, - vpImageMorphology::CONNEXITY_8); - printImage(I_test_flood_fill_8_connexity, - "I_test_flood_fill_8_connexity"); + vp::floodFill(I_test_flood_fill_8_connexity, vpImagePoint(2, 2), 0, 1, vpImageMorphology::CONNEXITY_8); + printImage(I_test_flood_fill_8_connexity, "I_test_flood_fill_8_connexity"); if (I_test_flood_fill_8_connexity != I_check_8_connexity) { - throw vpException(vpException::fatalError, - "Problem with vp::floodFill() and 8-connexity!"); + throw vpException(vpException::fatalError, "Problem with vp::floodFill() and 8-connexity!"); } std::cout << "\n(I_test_flood_fill_8_connexity == I_check_8_connexity)? " - << (I_test_flood_fill_8_connexity == I_check_8_connexity) - << std::endl; + << (I_test_flood_fill_8_connexity == I_check_8_connexity) << std::endl; // Read Klimt.ppm filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm"); vpImage I_klimt; vpImageIo::read(I_klimt, filename); - std::cout << "\nRead image: " << filename << " (" << I_klimt.getWidth() - << "x" << I_klimt.getHeight() << ")" << std::endl + std::cout << "\nRead image: " << filename << " (" << I_klimt.getWidth() << "x" << I_klimt.getHeight() << ")" + << std::endl << std::endl; - vpImageTools::binarise(I_klimt, (unsigned char)127, (unsigned char)255, - (unsigned char)0, (unsigned char)255, + vpImageTools::binarise(I_klimt, (unsigned char)127, (unsigned char)255, (unsigned char)0, (unsigned char)255, (unsigned char)255); int seed_x = 0; @@ -336,27 +310,19 @@ int main(int argc, const char **argv) vpImage I_klimt_flood_fill_4_connexity = I_klimt; double t = vpTime::measureTimeMs(); - vp::floodFill(I_klimt_flood_fill_4_connexity, - vpImagePoint(seed_y, seed_x), 0, 255, - vpImageMorphology::CONNEXITY_4); + vp::floodFill(I_klimt_flood_fill_4_connexity, vpImagePoint(seed_y, seed_x), 0, 255, vpImageMorphology::CONNEXITY_4); t = vpTime::measureTimeMs() - t; - std::cout << "Flood fill on Klimt image (4-connexity): " << t << " ms" - << std::endl; + std::cout << "Flood fill on Klimt image (4-connexity): " << t << " ms" << std::endl; - filename = - vpIoTools::createFilePath(opath, "Klimt_flood_fill_4_connexity.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_flood_fill_4_connexity.pgm"); vpImageIo::write(I_klimt_flood_fill_4_connexity, filename); vpImage I_klimt_flood_fill_8_connexity = I_klimt; t = vpTime::measureTimeMs(); - vp::floodFill(I_klimt_flood_fill_8_connexity, - vpImagePoint(seed_y, seed_x), 0, 255, - vpImageMorphology::CONNEXITY_8); + vp::floodFill(I_klimt_flood_fill_8_connexity, vpImagePoint(seed_y, seed_x), 0, 255, vpImageMorphology::CONNEXITY_8); t = vpTime::measureTimeMs() - t; - std::cout << "Flood fill on Klimt image (8-connexity): " << t << " ms" - << std::endl; - filename = - vpIoTools::createFilePath(opath, "Klimt_flood_fill_8_connexity.pgm"); + std::cout << "Flood fill on Klimt image (8-connexity): " << t << " ms" << std::endl; + filename = vpIoTools::createFilePath(opath, "Klimt_flood_fill_8_connexity.pgm"); vpImageIo::write(I_klimt_flood_fill_8_connexity, filename); #if VISP_HAVE_OPENCV_VERSION >= 0x020408 @@ -366,59 +332,45 @@ int main(int argc, const char **argv) // 4-connexity t = vpTime::measureTimeMs(); - cv::floodFill(matImg_klimt_4_connexity, cv::Point(seed_x, seed_y), - cv::Scalar(255), 0, cv::Scalar(), cv::Scalar(), 4); + cv::floodFill(matImg_klimt_4_connexity, cv::Point(seed_x, seed_y), cv::Scalar(255), 0, cv::Scalar(), cv::Scalar(), + 4); t = vpTime::measureTimeMs() - t; - std::cout << "OpenCV flood fill on Klimt image (4-connexity): " << t - << " ms" << std::endl; + std::cout << "OpenCV flood fill on Klimt image (4-connexity): " << t << " ms" << std::endl; vpImage I_klimt_flood_fill_4_connexity_check; - vpImageConvert::convert(matImg_klimt_4_connexity, - I_klimt_flood_fill_4_connexity_check); + vpImageConvert::convert(matImg_klimt_4_connexity, I_klimt_flood_fill_4_connexity_check); - filename = vpIoTools::createFilePath( - opath, "Klimt_flood_fill_4_connexity_opencv.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_flood_fill_4_connexity_opencv.pgm"); vpImageIo::write(I_klimt_flood_fill_4_connexity_check, filename); // 8-connexity t = vpTime::measureTimeMs(); - cv::floodFill(matImg_klimt_8_connexity, cv::Point(seed_x, seed_y), - cv::Scalar(255), 0, cv::Scalar(), cv::Scalar(), 8); + cv::floodFill(matImg_klimt_8_connexity, cv::Point(seed_x, seed_y), cv::Scalar(255), 0, cv::Scalar(), cv::Scalar(), + 8); t = vpTime::measureTimeMs() - t; - std::cout << "OpenCV flood fill on Klimt image (8-connexity): " << t - << " ms" << std::endl; + std::cout << "OpenCV flood fill on Klimt image (8-connexity): " << t << " ms" << std::endl; vpImage I_klimt_flood_fill_8_connexity_check; - vpImageConvert::convert(matImg_klimt_8_connexity, - I_klimt_flood_fill_8_connexity_check); + vpImageConvert::convert(matImg_klimt_8_connexity, I_klimt_flood_fill_8_connexity_check); - filename = vpIoTools::createFilePath( - opath, "Klimt_flood_fill_8_connexity_opencv.pgm"); + filename = vpIoTools::createFilePath(opath, "Klimt_flood_fill_8_connexity_opencv.pgm"); vpImageIo::write(I_klimt_flood_fill_8_connexity_check, filename); // Check std::cout << "\n(I_klimt_flood_fill_4_connexity == " "I_klimt_flood_fill_4_connexity_check)? " - << (I_klimt_flood_fill_4_connexity == - I_klimt_flood_fill_4_connexity_check) - << std::endl; + << (I_klimt_flood_fill_4_connexity == I_klimt_flood_fill_4_connexity_check) << std::endl; std::cout << "(I_klimt_flood_fill_8_connexity == " "I_klimt_flood_fill_8_connexity_check)? " - << (I_klimt_flood_fill_8_connexity == - I_klimt_flood_fill_8_connexity_check) - << std::endl; + << (I_klimt_flood_fill_8_connexity == I_klimt_flood_fill_8_connexity_check) << std::endl; - if (I_klimt_flood_fill_4_connexity != - I_klimt_flood_fill_4_connexity_check) { - throw vpException(vpException::fatalError, - "(I_klimt_flood_fill_4_connexity != " - "I_klimt_flood_fill_4_connexity_check)"); + if (I_klimt_flood_fill_4_connexity != I_klimt_flood_fill_4_connexity_check) { + throw vpException(vpException::fatalError, "(I_klimt_flood_fill_4_connexity != " + "I_klimt_flood_fill_4_connexity_check)"); } - if (I_klimt_flood_fill_8_connexity != - I_klimt_flood_fill_8_connexity_check) { - throw vpException(vpException::fatalError, - "(I_klimt_flood_fill_8_connexity != " - "I_klimt_flood_fill_8_connexity_check)"); + if (I_klimt_flood_fill_8_connexity != I_klimt_flood_fill_8_connexity_check) { + throw vpException(vpException::fatalError, "(I_klimt_flood_fill_8_connexity != " + "I_klimt_flood_fill_8_connexity_check)"); } #endif diff --git a/modules/imgproc/test/testImgproc.cpp b/modules/imgproc/test/testImgproc.cpp index 77d070af86..1ff70a177c 100644 --- a/modules/imgproc/test/testImgproc.cpp +++ b/modules/imgproc/test/testImgproc.cpp @@ -54,10 +54,8 @@ // 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); +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. @@ -68,8 +66,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, \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) +void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user) { fprintf(stdout, "\n\ Test imgproc functions.\n\ @@ -112,8 +109,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user) { const char *optarg_; int c; @@ -205,8 +201,7 @@ int main(int argc, const char **argv) 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; + std::cerr << " Check your -o " << opt_opath << " option " << std::endl; return EXIT_FAILURE; } } @@ -217,8 +212,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -227,13 +221,10 @@ int main(int argc, const char **argv) 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 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; + std::cerr << " Use -i 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; return EXIT_FAILURE; } @@ -250,8 +241,7 @@ int main(int argc, const char **argv) vpImage I_color; std::cout << "Read image: " << filename << std::endl; vpImageIo::read(I_color, filename); - std::cout << "Image: " << I_color.getWidth() << "x" << I_color.getHeight() - << std::endl; + std::cout << "Image: " << I_color.getWidth() << "x" << I_color.getHeight() << std::endl; // Adjust double alpha = 1.5, beta = -10.0; @@ -270,12 +260,10 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); vp::equalizeHistogram(I_color, I_color_equalize_histogram); t = vpTime::measureTimeMs() - t; - std::cout << "Time to do color histogram equalization: " << t << " ms" - << std::endl; + std::cout << "Time to do color histogram equalization: " << t << " ms" << std::endl; // Save equalizeHistogram - filename = - vpIoTools::createFilePath(opath, "Klimt_equalize_histogram.ppm"); + filename = vpIoTools::createFilePath(opath, "Klimt_equalize_histogram.ppm"); vpImageIo::write(I_color_equalize_histogram, filename); // Gamma correction @@ -284,8 +272,7 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); vp::gammaCorrection(I_color, I_color_gamma_correction, gamma); t = vpTime::measureTimeMs() - t; - std::cout << "Time to do color gamma correction: " << t << " ms" - << std::endl; + std::cout << "Time to do color gamma correction: " << t << " ms" << std::endl; // Save gammaCorrection filename = vpIoTools::createFilePath(opath, "Klimt_gamma_correction.ppm"); @@ -307,8 +294,7 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); vp::stretchContrast(I_color, I_color_stretch_contrast); t = vpTime::measureTimeMs() - t; - std::cout << "Time to do color contrast stretching: " << t << " ms" - << std::endl; + std::cout << "Time to do color contrast stretching: " << t << " ms" << std::endl; // Save stretchContrast filename = vpIoTools::createFilePath(opath, "Klimt_stretch_contrast.ppm"); @@ -319,12 +305,10 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); vp::stretchContrastHSV(I_color, I_color_stretch_contrast_HSV); t = vpTime::measureTimeMs() - t; - std::cout << "Time to do color HSV contrast stretching: " << t << " ms" - << std::endl; + std::cout << "Time to do color HSV contrast stretching: " << t << " ms" << std::endl; // Save stretchContrastHSV - filename = - vpIoTools::createFilePath(opath, "Klimt_stretch_contrast_HSV.ppm"); + filename = vpIoTools::createFilePath(opath, "Klimt_stretch_contrast_HSV.ppm"); vpImageIo::write(I_color_stretch_contrast_HSV, filename); // Unsharp Mask @@ -358,8 +342,7 @@ int main(int argc, const char **argv) vpImage I; std::cout << "\nRead image: " << filename << std::endl; vpImageIo::read(I, filename); - std::cout << "Image: " << I.getWidth() << "x" << I.getHeight() - << std::endl; + std::cout << "Image: " << I.getWidth() << "x" << I.getHeight() << std::endl; // Adjust vpImage I_adjust; @@ -378,12 +361,10 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); vp::equalizeHistogram(I, I_equalize_histogram); t = vpTime::measureTimeMs() - t; - std::cout << "Time to do grayscale histogram equalization: " << t << " ms" - << std::endl; + std::cout << "Time to do grayscale histogram equalization: " << t << " ms" << std::endl; // Save equalizeHistogram - filename = - vpIoTools::createFilePath(opath, "image0000_equalize_histogram.pgm"); + filename = vpIoTools::createFilePath(opath, "image0000_equalize_histogram.pgm"); vpImageIo::write(I_equalize_histogram, filename); // Gamma correction @@ -392,12 +373,10 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); vp::gammaCorrection(I, I_gamma_correction, gamma); t = vpTime::measureTimeMs() - t; - std::cout << "Time to do grayscale gamma correction: " << t << " ms" - << std::endl; + std::cout << "Time to do grayscale gamma correction: " << t << " ms" << std::endl; // Save gammaCorrection - filename = - vpIoTools::createFilePath(opath, "image0000_gamma_correction.pgm"); + filename = vpIoTools::createFilePath(opath, "image0000_gamma_correction.pgm"); vpImageIo::write(I_gamma_correction, filename); // Stretch contrast @@ -405,12 +384,10 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); vp::stretchContrast(I, I_stretch_contrast); t = vpTime::measureTimeMs() - t; - std::cout << "Time to do grayscale contrast stretching: " << t << " ms" - << std::endl; + std::cout << "Time to do grayscale contrast stretching: " << t << " ms" << std::endl; // Save stretchContrast - filename = - vpIoTools::createFilePath(opath, "image0000_stretch_contrast.pgm"); + filename = vpIoTools::createFilePath(opath, "image0000_stretch_contrast.pgm"); vpImageIo::write(I_stretch_contrast, filename); // Unsharp Mask @@ -418,8 +395,7 @@ int main(int argc, const char **argv) t = vpTime::measureTimeMs(); vp::unsharpMask(I, I_unsharp_mask); t = vpTime::measureTimeMs() - t; - std::cout << "Time to do grayscale unsharp mask: " << t << " ms" - << std::endl; + std::cout << "Time to do grayscale unsharp mask: " << t << " ms" << std::endl; // Save unsharpMask filename = vpIoTools::createFilePath(opath, "image0000_unsharp_mask.pgm"); diff --git a/modules/io/include/visp3/io/vpDiskGrabber.h b/modules/io/include/visp3/io/vpDiskGrabber.h index 99bf1df0b8..5d0d668ade 100644 --- a/modules/io/include/visp3/io/vpDiskGrabber.h +++ b/modules/io/include/visp3/io/vpDiskGrabber.h @@ -124,8 +124,7 @@ class VISP_EXPORT vpDiskGrabber : public vpFrameGrabber public: vpDiskGrabber(); explicit vpDiskGrabber(const std::string &genericName); - explicit vpDiskGrabber(const std::string &dir, const std::string &basename, - long number, int step, unsigned int noz, + explicit vpDiskGrabber(const std::string &dir, const std::string &basename, long number, int step, unsigned int noz, const std::string &ext); virtual ~vpDiskGrabber(); diff --git a/modules/io/include/visp3/io/vpImageIo.h b/modules/io/include/visp3/io/vpImageIo.h index 5c56fc30dc..07aacb9bb0 100644 --- a/modules/io/include/visp3/io/vpImageIo.h +++ b/modules/io/include/visp3/io/vpImageIo.h @@ -107,9 +107,8 @@ int main() 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 + 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 @@ -144,8 +143,7 @@ class VISP_EXPORT vpImageIo static void read(vpImage &I, const std::string &filename); static void read(vpImage &I, const std::string &filename); - static void write(const vpImage &I, - const std::string &filename); + static void write(const vpImage &I, const std::string &filename); static void write(const vpImage &I, const std::string &filename); static void readPFM(vpImage &I, const std::string &filename); @@ -157,8 +155,7 @@ class VISP_EXPORT vpImageIo static void readPPM(vpImage &I, const std::string &filename); #if (defined(VISP_HAVE_JPEG) || defined(VISP_HAVE_OPENCV)) - static void readJPEG(vpImage &I, - const std::string &filename); + static void readJPEG(vpImage &I, const std::string &filename); static void readJPEG(vpImage &I, const std::string &filename); #endif @@ -169,25 +166,20 @@ class VISP_EXPORT vpImageIo static void writePFM(const vpImage &I, const std::string &filename); - static void writePGM(const vpImage &I, - const std::string &filename); + static void writePGM(const vpImage &I, const std::string &filename); static void writePGM(const vpImage &I, const std::string &filename); static void writePGM(const vpImage &I, const std::string &filename); - static void writePPM(const vpImage &I, - const std::string &filename); + static void writePPM(const vpImage &I, const std::string &filename); static void writePPM(const vpImage &I, const std::string &filename); #if (defined(VISP_HAVE_JPEG) || defined(VISP_HAVE_OPENCV)) - static void writeJPEG(const vpImage &I, - const std::string &filename); - static void writeJPEG(const vpImage &I, - const std::string &filename); + static void writeJPEG(const vpImage &I, const std::string &filename); + static void writeJPEG(const vpImage &I, const std::string &filename); #endif #if (defined(VISP_HAVE_PNG) || defined(VISP_HAVE_OPENCV)) - static void writePNG(const vpImage &I, - const std::string &filename); + static void writePNG(const vpImage &I, const std::string &filename); static void writePNG(const vpImage &I, const std::string &filename); #endif }; diff --git a/modules/io/include/visp3/io/vpKeyboard.h b/modules/io/include/visp3/io/vpKeyboard.h index 31af7d61a9..238a259a84 100644 --- a/modules/io/include/visp3/io/vpKeyboard.h +++ b/modules/io/include/visp3/io/vpKeyboard.h @@ -46,8 +46,7 @@ #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) #include #include diff --git a/modules/io/include/visp3/io/vpParallelPortException.h b/modules/io/include/visp3/io/vpParallelPortException.h index 538e714615..7a14a7065d 100644 --- a/modules/io/include/visp3/io/vpParallelPortException.h +++ b/modules/io/include/visp3/io/vpParallelPortException.h @@ -97,11 +97,7 @@ class VISP_EXPORT vpParallelPortException : public vpException setMessage(format, args); va_end(args); } - vpParallelPortException(const int id, const std::string &msg) - : vpException(id, msg) - { - ; - } + vpParallelPortException(const int id, const std::string &msg) : vpException(id, msg) { ; } explicit vpParallelPortException(const int id) : vpException(id) { ; } }; diff --git a/modules/io/include/visp3/io/vpParseArgv.h b/modules/io/include/visp3/io/vpParseArgv.h index 4ff96e5685..a6410ca944 100644 --- a/modules/io/include/visp3/io/vpParseArgv.h +++ b/modules/io/include/visp3/io/vpParseArgv.h @@ -49,8 +49,8 @@ #include // Usage : [-bool] [-int ] [-long ] -// [-float ] [-double ] [-string ] [-h] int main(int argc, const char ** argv) +// [-float ] [-double ] [-string ] [-h] +int main(int argc, const char ** argv) { // Variables to set by command line parsing bool b_val = false; @@ -77,8 +77,7 @@ value>] [-h] int main(int argc, const char ** argv) "A string value."}, {"-h", vpParseArgv::ARGV_HELP, (char*) NULL, (char *) NULL, "Print the help."}, - {(char*) NULL, vpParseArgv::ARGV_END, (char*) NULL, (char*) NULL, (char*) -NULL} } ; + {(char*) NULL, vpParseArgv::ARGV_END, (char*) NULL, (char*) NULL, (char*) NULL} } ; // Read the command line options if(vpParseArgv::parse(&argc, argv, argTable, @@ -101,8 +100,7 @@ NULL} } ; #include // List of allowed command line options -#define GETOPTARGS "bi:l:f:d:h" // double point mean here that the -preceding option request an argument +#define GETOPTARGS "bi:l:f:d:h" // double point mean here that the preceding option request an argument // Usage : [-b] [-i ] [-l ] // [-f ] [-d ] [-s ] [-h] @@ -154,9 +152,9 @@ class VISP_EXPORT vpParseArgv Legal values for the type field of a vpArgvInfo. */ typedef enum { - ARGV_CONSTANT, /*!< Stand alone argument. Same as ARGV_CONSTANT_INT. */ - ARGV_CONSTANT_INT, /*!< Stand alone argument associated to an int var that - is set to 1. */ + ARGV_CONSTANT, /*!< Stand alone argument. Same as ARGV_CONSTANT_INT. */ + ARGV_CONSTANT_INT, /*!< Stand alone argument associated to an int var that + is set to 1. */ ARGV_CONSTANT_BOOL, /*!< Stand alone argument associated to a bool var that is set to true. */ ARGV_INT, /*!< Argument is associated to an int. */ @@ -175,12 +173,12 @@ class VISP_EXPORT vpParseArgv 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_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; @@ -204,10 +202,8 @@ class VISP_EXPORT vpParseArgv 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); + 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); diff --git a/modules/io/include/visp3/io/vpVideoReader.h b/modules/io/include/visp3/io/vpVideoReader.h index 6fd763ff70..669628b6f1 100644 --- a/modules/io/include/visp3/io/vpVideoReader.h +++ b/modules/io/include/visp3/io/vpVideoReader.h @@ -107,8 +107,8 @@ int main() // 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); + // After positionning the video reader use acquire to read the video frame by frame + reader.acquire(I); return 0; #endif @@ -146,7 +146,9 @@ int main() \endcode Note that it is also possible to access to a specific frame using -getFrame(). \code #include +getFrame(). +\code +#include int main() { @@ -371,10 +373,7 @@ class VISP_EXPORT vpVideoReader : public vpFrameGrabber \sa setFrameStep() */ - inline void setFrameStep(const long frame_step) - { - this->frameStep = frame_step; - } + inline void setFrameStep(const long frame_step) { this->frameStep = frame_step; } private: vpVideoFormatType getFormat(const char *filename); @@ -383,8 +382,7 @@ class VISP_EXPORT vpVideoReader : public vpFrameGrabber void findLastFrameIndex(); bool isImageExtensionSupported(); bool isVideoExtensionSupported(); - long extractImageIndex(const std::string &imageName, - const std::string &format); + long extractImageIndex(const std::string &imageName, const std::string &format); bool checkImageNameFormat(const std::string &format); void getProperties(); }; diff --git a/modules/io/include/visp3/io/vpVideoWriter.h b/modules/io/include/visp3/io/vpVideoWriter.h index af973980c2..09a76a44a6 100644 --- a/modules/io/include/visp3/io/vpVideoWriter.h +++ b/modules/io/include/visp3/io/vpVideoWriter.h @@ -222,10 +222,7 @@ class VISP_EXPORT vpVideoWriter void saveFrame(vpImage &I); #if VISP_HAVE_OPENCV_VERSION >= 0x020100 - inline void setCodec(const int fourcc_codec) - { - this->fourcc = fourcc_codec; - } + inline void setCodec(const int fourcc_codec) { this->fourcc = fourcc_codec; } #endif void setFileName(const char *filename); @@ -235,10 +232,7 @@ class VISP_EXPORT vpVideoWriter \param first_frame : The first frame index. */ - inline void setFirstFrameIndex(const unsigned int first_frame) - { - this->firstFrame = first_frame; - } + inline void setFirstFrameIndex(const unsigned int first_frame) { this->firstFrame = first_frame; } #if VISP_HAVE_OPENCV_VERSION >= 0x020100 /*! Sets the framerate in Hz of the video when encoding. @@ -247,10 +241,7 @@ class VISP_EXPORT vpVideoWriter By default the framerate is set to 25Hz. */ - inline void setFramerate(const double frame_rate) - { - this->framerate = frame_rate; - } + inline void setFramerate(const double frame_rate) { this->framerate = frame_rate; } #endif private: diff --git a/modules/io/src/image/vpImageIo.cpp b/modules/io/src/image/vpImageIo.cpp index dfb76a98bd..007b239750 100644 --- a/modules/io/src/image/vpImageIo.cpp +++ b/modules/io/src/image/vpImageIo.cpp @@ -46,8 +46,7 @@ #include #include -void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, - const std::string &magic, unsigned int &w, +void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, const std::string &magic, unsigned int &w, unsigned int &h, unsigned int &maxval); #ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -60,47 +59,37 @@ void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, * \param h[out] : Image height. * \param maxval[out] : Maximum pixel value. */ -void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, - const std::string &magic, unsigned int &w, +void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, const std::string &magic, unsigned int &w, unsigned int &h, unsigned int &maxval) { std::string line; unsigned int nb_elt = 4, cpt_elt = 0; while (cpt_elt != nb_elt) { // Skip empty lines or lines starting with # (comment) - while (std::getline(fd, line) && - (line.compare(0, 1, "#") == 0 || line.size() == 0)) { + while (std::getline(fd, line) && (line.compare(0, 1, "#") == 0 || line.size() == 0)) { }; if (fd.eof()) { fd.close(); - throw(vpImageException(vpImageException::ioError, - "Cannot read header of file \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot read header of file \"%s\"", filename.c_str())); } - std::vector header = - vpIoTools::splitChain(line, std::string(" ")); + std::vector header = vpIoTools::splitChain(line, std::string(" ")); if (header.size() == 0) { fd.close(); - throw(vpImageException(vpImageException::ioError, - "Cannot read header of file \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot read header of file \"%s\"", filename.c_str())); } if (cpt_elt == 0) { // decode magic if (header[0].compare(0, magic.size(), magic) != 0) { fd.close(); - throw( - vpImageException(vpImageException::ioError, - "\"%s\" is not a PNM file with magic number %s", - filename.c_str(), magic.c_str())); + throw(vpImageException(vpImageException::ioError, "\"%s\" is not a PNM file with magic number %s", + filename.c_str(), magic.c_str())); } cpt_elt++; header.erase(header.begin(), - header.begin() + - 1); // erase first element that is processed + header.begin() + 1); // erase first element that is processed } while (header.size()) { if (cpt_elt == 1) { // decode width @@ -108,22 +97,19 @@ void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, ss >> w; cpt_elt++; header.erase(header.begin(), - header.begin() + - 1); // erase first element that is processed - } else if (cpt_elt == 2) { // decode height + header.begin() + 1); // erase first element that is processed + } else if (cpt_elt == 2) { // decode height std::istringstream ss(header[0]); ss >> h; cpt_elt++; header.erase(header.begin(), - header.begin() + - 1); // erase first element that is processed - } else if (cpt_elt == 3) { // decode maxval + header.begin() + 1); // erase first element that is processed + } else if (cpt_elt == 3) { // decode maxval std::istringstream ss(header[0]); ss >> maxval; cpt_elt++; header.erase(header.begin(), - header.begin() + - 1); // erase first element that is processed + header.begin() + 1); // erase first element that is processed } } } @@ -222,8 +208,7 @@ void vpImageIo::read(vpImage &I, const std::string &filename) { bool exist = vpIoTools::checkFilename(filename); if (!exist) { - std::string message = - "Cannot read file: \"" + std::string(filename) + "\" doesn't exist"; + std::string message = "Cannot read file: \"" + std::string(filename) + "\" doesn't exist"; throw(vpImageException(vpImageException::ioError, message)); } @@ -269,9 +254,7 @@ void vpImageIo::read(vpImage &I, const std::string &filename) // std::cout << "Use opencv to read the image" << std::endl; cv::Mat cvI = cv::imread(final_filename, cv::IMREAD_GRAYSCALE); if (cvI.cols == 0 && cvI.rows == 0) { - std::string message = "Cannot read file \"" + - std::string(final_filename) + - "\": Image format not supported"; + std::string message = "Cannot read file \"" + std::string(final_filename) + "\": Image format not supported"; throw(vpImageException(vpImageException::ioError, message)); } vpImageConvert::convert(cvI, I); @@ -279,16 +262,12 @@ void vpImageIo::read(vpImage &I, const std::string &filename) // std::cout << "Use opencv to read the image" << std::endl; cv::Mat cvI = cv::imread(final_filename, CV_LOAD_IMAGE_GRAYSCALE); if (cvI.cols == 0 && cvI.rows == 0) { - std::string message = "Cannot read file \"" + - std::string(final_filename) + - "\": Image format not supported"; + std::string message = "Cannot read file \"" + std::string(final_filename) + "\": Image format not supported"; throw(vpImageException(vpImageException::ioError, message)); } vpImageConvert::convert(cvI, I); #else - std::string message = "Cannot read file \"" + - std::string(final_filename) + - "\": Image format not supported"; + std::string message = "Cannot read file \"" + std::string(final_filename) + "\": Image format not supported"; throw(vpImageException(vpImageException::ioError, message)); #endif } @@ -316,8 +295,7 @@ void vpImageIo::read(vpImage &I, const std::string &filename) { bool exist = vpIoTools::checkFilename(filename); if (!exist) { - std::string message = - "Cannot read file: \"" + std::string(filename) + "\" doesn't exist"; + std::string message = "Cannot read file: \"" + std::string(filename) + "\" doesn't exist"; throw(vpImageException(vpImageException::ioError, message)); } // Allows to use ~ symbol or env variables in path @@ -362,9 +340,7 @@ void vpImageIo::read(vpImage &I, const std::string &filename) // std::cout << "Use opencv to read the image" << std::endl; cv::Mat cvI = cv::imread(final_filename, cv::IMREAD_COLOR); if (cvI.cols == 0 && cvI.rows == 0) { - std::string message = "Cannot read file \"" + - std::string(final_filename) + - "\": Image format not supported"; + std::string message = "Cannot read file \"" + std::string(final_filename) + "\": Image format not supported"; throw(vpImageException(vpImageException::ioError, message)); } vpImageConvert::convert(cvI, I); @@ -372,16 +348,12 @@ void vpImageIo::read(vpImage &I, const std::string &filename) // std::cout << "Use opencv to read the image" << std::endl; cv::Mat cvI = cv::imread(final_filename, CV_LOAD_IMAGE_COLOR); if (cvI.cols == 0 && cvI.rows == 0) { - std::string message = "Cannot read file \"" + - std::string(final_filename) + - "\": Image format not supported"; + std::string message = "Cannot read file \"" + std::string(final_filename) + "\": Image format not supported"; throw(vpImageException(vpImageException::ioError, message)); } vpImageConvert::convert(cvI, I); #else - std::string message = "Cannot read file \"" + - std::string(final_filename) + - "\": Image format not supported"; + std::string message = "Cannot read file \"" + std::string(final_filename) + "\": Image format not supported"; throw(vpImageException(vpImageException::ioError, message)); #endif } @@ -400,8 +372,7 @@ void vpImageIo::read(vpImage &I, const std::string &filename) \param I : Image to write. \param filename : Name of the file containing the image. */ -void vpImageIo::write(const vpImage &I, - const std::string &filename) +void vpImageIo::write(const vpImage &I, const std::string &filename) { bool try_opencv_writer = false; @@ -444,10 +415,8 @@ void vpImageIo::write(const vpImage &I, 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")); + vpCERROR << "Cannot write file: Image format not supported..." << std::endl; + throw(vpImageException(vpImageException::ioError, "Cannot write file: Image format not supported")); #endif } } @@ -508,10 +477,8 @@ void vpImageIo::write(const vpImage &I, const std::string &filename) 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")); + vpCERROR << "Cannot write file: Image format not supported..." << std::endl; + throw(vpImageException(vpImageException::ioError, "Cannot write file: Image format not supported")); #endif } } @@ -535,16 +502,13 @@ void vpImageIo::writePFM(const vpImage &I, const std::string &filename) // Test the filename if (filename.empty()) { - throw(vpImageException(vpImageException::ioError, - "Cannot write PFM image: filename empty")); + throw(vpImageException(vpImageException::ioError, "Cannot write PFM image: filename empty")); } fd = fopen(filename.c_str(), "wb"); if (fd == NULL) { - throw(vpImageException(vpImageException::ioError, - "Cannot create PFM file \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot create PFM file \"%s\"", filename.c_str())); } // Write the head @@ -559,10 +523,8 @@ void vpImageIo::writePFM(const vpImage &I, const std::string &filename) ierr = fwrite(I.bitmap, sizeof(float), nbyte, fd); if (ierr != nbyte) { fclose(fd); - throw(vpImageException( - vpImageException::ioError, - "Cannot save PFM file \"%s\": only %d bytes over %d saved ", - filename.c_str(), ierr, nbyte)); + throw(vpImageException(vpImageException::ioError, "Cannot save PFM file \"%s\": only %d bytes over %d saved ", + filename.c_str(), ierr, nbyte)); } fflush(fd); @@ -580,24 +542,20 @@ void vpImageIo::writePFM(const vpImage &I, const std::string &filename) \param filename : Name of the file containing the image. */ -void vpImageIo::writePGM(const vpImage &I, - const std::string &filename) +void vpImageIo::writePGM(const vpImage &I, const std::string &filename) { FILE *fd; // Test the filename if (filename.empty()) { - throw(vpImageException(vpImageException::ioError, - "Cannot create PGM file: filename empty")); + throw(vpImageException(vpImageException::ioError, "Cannot create PGM file: filename empty")); } fd = fopen(filename.c_str(), "wb"); if (fd == NULL) { - throw(vpImageException(vpImageException::ioError, - "Cannot create PGM file \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot create PGM file \"%s\"", filename.c_str())); } // Write the head @@ -612,10 +570,8 @@ void vpImageIo::writePGM(const vpImage &I, ierr = fwrite(I.bitmap, sizeof(unsigned char), nbyte, fd); if (ierr != nbyte) { fclose(fd); - throw(vpImageException( - vpImageException::ioError, - "Cannot save PGM file \"%s\": only %d over %d bytes saved", - filename.c_str(), ierr, nbyte)); + throw(vpImageException(vpImageException::ioError, "Cannot save PGM file \"%s\": only %d over %d bytes saved", + filename.c_str(), ierr, nbyte)); } fflush(fd); @@ -651,24 +607,20 @@ void vpImageIo::writePGM(const vpImage &I, const std::string &filename) \param filename : Name of the file containing the image. */ -void vpImageIo::writePGM(const vpImage &I, - const std::string &filename) +void vpImageIo::writePGM(const vpImage &I, const std::string &filename) { FILE *fd; // Test the filename if (filename.empty()) { - throw(vpImageException(vpImageException::ioError, - "Cannot create PGM file: filename empty")); + throw(vpImageException(vpImageException::ioError, "Cannot create PGM file: filename empty")); } fd = fopen(filename.c_str(), "wb"); if (fd == NULL) { - throw(vpImageException(vpImageException::ioError, - "Cannot create PGM file \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot create PGM file \"%s\"", filename.c_str())); } // Write the head @@ -686,10 +638,8 @@ void vpImageIo::writePGM(const vpImage &I, ierr = fwrite(Itmp.bitmap, sizeof(unsigned char), nbyte, fd); if (ierr != nbyte) { fclose(fd); - throw(vpImageException( - vpImageException::ioError, - "Cannot save PGM file \"%s\": only %d over %d bytes saved", - filename.c_str(), ierr, nbyte)); + throw(vpImageException(vpImageException::ioError, "Cannot save PGM file \"%s\": only %d over %d bytes saved", + filename.c_str(), ierr, nbyte)); } fflush(fd); @@ -722,21 +672,18 @@ void vpImageIo::readPFM(vpImage &I, const std::string &filename) // Open the filename if (!fd.is_open()) { - throw(vpImageException(vpImageException::ioError, - "Cannot open file \"%s\"", filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot open file \"%s\"", filename.c_str())); } vp_decodeHeaderPNM(filename, fd, magic, w, h, maxval); if (w > w_max || h > h_max) { fd.close(); - throw(vpException(vpException::badValue, "Bad image size in \"%s\"", - filename.c_str())); + throw(vpException(vpException::badValue, "Bad image size in \"%s\"", filename.c_str())); } if (maxval > maxval_max) { fd.close(); - throw(vpImageException(vpImageException::ioError, "Bad maxval in \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Bad maxval in \"%s\"", filename.c_str())); } if ((h != I.getHeight()) || (w != I.getWidth())) { @@ -747,9 +694,8 @@ void vpImageIo::readPFM(vpImage &I, const std::string &filename) fd.read((char *)I.bitmap, sizeof(float) * nbyte); if (!fd) { fd.close(); - throw(vpImageException(vpImageException::ioError, - "Read only %d of %d bytes in file \"%s\"", - fd.gcount(), nbyte, filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Read only %d of %d bytes in file \"%s\"", fd.gcount(), nbyte, + filename.c_str())); } fd.close(); @@ -770,8 +716,7 @@ void vpImageIo::readPFM(vpImage &I, const std::string &filename) \param filename : Name of the file containing the image. */ -void vpImageIo::readPGM(vpImage &I, - const std::string &filename) +void vpImageIo::readPGM(vpImage &I, const std::string &filename) { unsigned int w = 0, h = 0, maxval = 0; unsigned int w_max = 100000, h_max = 100000, maxval_max = 255; @@ -781,21 +726,18 @@ void vpImageIo::readPGM(vpImage &I, // Open the filename if (!fd.is_open()) { - throw(vpImageException(vpImageException::ioError, - "Cannot open file \"%s\"", filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot open file \"%s\"", filename.c_str())); } vp_decodeHeaderPNM(filename, fd, magic, w, h, maxval); if (w > w_max || h > h_max) { fd.close(); - throw(vpException(vpException::badValue, "Bad image size in \"%s\"", - filename.c_str())); + throw(vpException(vpException::badValue, "Bad image size in \"%s\"", filename.c_str())); } if (maxval > maxval_max) { fd.close(); - throw(vpImageException(vpImageException::ioError, "Bad maxval in \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Bad maxval in \"%s\"", filename.c_str())); } if ((h != I.getHeight()) || (w != I.getWidth())) { @@ -806,9 +748,8 @@ void vpImageIo::readPGM(vpImage &I, fd.read((char *)I.bitmap, nbyte); if (!fd) { fd.close(); - throw(vpImageException(vpImageException::ioError, - "Read only %d of %d bytes in file \"%s\"", - fd.gcount(), nbyte, filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Read only %d of %d bytes in file \"%s\"", fd.gcount(), nbyte, + filename.c_str())); } fd.close(); @@ -861,8 +802,7 @@ void vpImageIo::readPGM(vpImage &I, const std::string &filename) \param filename : Name of the file containing the image. */ -void vpImageIo::readPPM(vpImage &I, - const std::string &filename) +void vpImageIo::readPPM(vpImage &I, const std::string &filename) { vpImage Itmp; @@ -892,21 +832,18 @@ void vpImageIo::readPPM(vpImage &I, const std::string &filename) // Open the filename if (!fd.is_open()) { - throw(vpImageException(vpImageException::ioError, - "Cannot open file \"%s\"", filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot open file \"%s\"", filename.c_str())); } vp_decodeHeaderPNM(filename, fd, magic, w, h, maxval); if (w > w_max || h > h_max) { fd.close(); - throw(vpException(vpException::badValue, "Bad image size in \"%s\"", - filename.c_str())); + throw(vpException(vpException::badValue, "Bad image size in \"%s\"", filename.c_str())); } if (maxval > maxval_max) { fd.close(); - throw(vpImageException(vpImageException::ioError, "Bad maxval in \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Bad maxval in \"%s\"", filename.c_str())); } if ((h != I.getHeight()) || (w != I.getWidth())) { @@ -920,10 +857,8 @@ void vpImageIo::readPPM(vpImage &I, const std::string &filename) if (!fd) { fd.close(); - throw(vpImageException(vpImageException::ioError, - "Read only %d of %d bytes in file \"%s\"", - (i * I.getWidth() + j) * 3 + fd.gcount(), - I.getSize() * 3, filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Read only %d of %d bytes in file \"%s\"", + (i * I.getWidth() + j) * 3 + fd.gcount(), I.getSize() * 3, filename.c_str())); } I[i][j].R = rgb[0]; @@ -946,8 +881,7 @@ void vpImageIo::readPPM(vpImage &I, const std::string &filename) */ -void vpImageIo::writePPM(const vpImage &I, - const std::string &filename) +void vpImageIo::writePPM(const vpImage &I, const std::string &filename) { vpImage Itmp; @@ -963,23 +897,19 @@ void vpImageIo::writePPM(const vpImage &I, \param I : Image to save as a (PPM P6) file. \param filename : Name of the file containing the image. */ -void vpImageIo::writePPM(const vpImage &I, - const std::string &filename) +void vpImageIo::writePPM(const vpImage &I, const std::string &filename) { FILE *f; // Test the filename if (filename.empty()) { - throw(vpImageException(vpImageException::ioError, - "Cannot create PPM file: filename empty")); + throw(vpImageException(vpImageException::ioError, "Cannot create PPM file: filename empty")); } f = fopen(filename.c_str(), "wb"); if (f == NULL) { - throw(vpImageException(vpImageException::ioError, - "Cannot create PPM file \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot create PPM file \"%s\"", filename.c_str())); } fprintf(f, "P6\n"); // Magic number @@ -997,8 +927,7 @@ void vpImageIo::writePPM(const vpImage &I, size_t res = fwrite(&rgb, 1, 3, f); if (res != 3) { fclose(f); - throw(vpImageException(vpImageException::ioError, - "cannot write file \"%s\"", filename.c_str())); + throw(vpImageException(vpImageException::ioError, "cannot write file \"%s\"", filename.c_str())); } } } @@ -1020,8 +949,7 @@ void vpImageIo::writePPM(const vpImage &I, \param I : Image to save as a JPEG file. \param filename : Name of the file containing the image. */ -void vpImageIo::writeJPEG(const vpImage &I, - const std::string &filename) +void vpImageIo::writeJPEG(const vpImage &I, const std::string &filename) { struct jpeg_compress_struct cinfo; struct jpeg_error_mgr jerr; @@ -1032,16 +960,13 @@ void vpImageIo::writeJPEG(const vpImage &I, // Test the filename if (filename.empty()) { - throw(vpImageException(vpImageException::ioError, - "Cannot create JPEG file: filename empty")); + throw(vpImageException(vpImageException::ioError, "Cannot create JPEG file: filename empty")); } file = fopen(filename.c_str(), "wb"); if (file == NULL) { - throw(vpImageException(vpImageException::ioError, - "Cannot create JPEG file \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot create JPEG file \"%s\"", filename.c_str())); } unsigned int width = I.getWidth(); @@ -1081,8 +1006,7 @@ void vpImageIo::writeJPEG(const vpImage &I, \param I : Image to save as a JPEG file. \param filename : Name of the file containing the image. */ -void vpImageIo::writeJPEG(const vpImage &I, - const std::string &filename) +void vpImageIo::writeJPEG(const vpImage &I, const std::string &filename) { struct jpeg_compress_struct cinfo; struct jpeg_error_mgr jerr; @@ -1093,16 +1017,13 @@ void vpImageIo::writeJPEG(const vpImage &I, // Test the filename if (filename.empty()) { - throw(vpImageException(vpImageException::ioError, - "Cannot create JPEG file: filename empty")); + throw(vpImageException(vpImageException::ioError, "Cannot create JPEG file: filename empty")); } file = fopen(filename.c_str(), "wb"); if (file == NULL) { - throw(vpImageException(vpImageException::ioError, - "Cannot create JPEG file \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot create JPEG file \"%s\"", filename.c_str())); } unsigned int width = I.getWidth(); @@ -1156,8 +1077,7 @@ void vpImageIo::writeJPEG(const vpImage &I, \param filename : Name of the file containing the image. */ -void vpImageIo::readJPEG(vpImage &I, - const std::string &filename) +void vpImageIo::readJPEG(vpImage &I, const std::string &filename) { struct jpeg_decompress_struct cinfo; struct jpeg_error_mgr jerr; @@ -1168,15 +1088,13 @@ void vpImageIo::readJPEG(vpImage &I, // Test the filename if (filename.empty()) { - throw(vpImageException(vpImageException::ioError, - "Cannot read JPEG image: filename empty")); + throw(vpImageException(vpImageException::ioError, "Cannot read JPEG image: filename empty")); } file = fopen(filename.c_str(), "rb"); if (file == NULL) { - throw(vpImageException(vpImageException::ioError, - "Cannot read JPEG file \"%s\"", filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot read JPEG file \"%s\"", filename.c_str())); } jpeg_stdio_src(&cinfo, file); @@ -1190,10 +1108,8 @@ void vpImageIo::readJPEG(vpImage &I, 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); + 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 Ic(height, width); @@ -1252,15 +1168,13 @@ void vpImageIo::readJPEG(vpImage &I, const std::string &filename) // Test the filename if (filename.empty()) { - throw(vpImageException(vpImageException::ioError, - "Cannot read JPEG image: filename empty")); + throw(vpImageException(vpImageException::ioError, "Cannot read JPEG image: filename empty")); } file = fopen(filename.c_str(), "rb"); if (file == NULL) { - throw(vpImageException(vpImageException::ioError, - "Cannot read JPEG file \"%s\"", filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot read JPEG file \"%s\"", filename.c_str())); } jpeg_stdio_src(&cinfo, file); @@ -1275,10 +1189,8 @@ void vpImageIo::readJPEG(vpImage &I, const std::string &filename) 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); + 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; @@ -1319,8 +1231,7 @@ void vpImageIo::readJPEG(vpImage &I, const std::string &filename) \param I : Image to save as a JPEG file. \param filename : Name of the file containing the image. */ -void vpImageIo::writeJPEG(const vpImage &I, - const std::string &filename) +void vpImageIo::writeJPEG(const vpImage &I, const std::string &filename) { #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) cv::Mat Ip; @@ -1343,8 +1254,7 @@ void vpImageIo::writeJPEG(const vpImage &I, \param I : Image to save as a JPEG file. \param filename : Name of the file containing the image. */ -void vpImageIo::writeJPEG(const vpImage &I, - const std::string &filename) +void vpImageIo::writeJPEG(const vpImage &I, const std::string &filename) { #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) cv::Mat Ip; @@ -1376,31 +1286,27 @@ void vpImageIo::writeJPEG(const vpImage &I, \param filename : Name of the file containing the image. */ -void vpImageIo::readJPEG(vpImage &I, - const std::string &filename) +void vpImageIo::readJPEG(vpImage &I, const std::string &filename) { #if (VISP_HAVE_OPENCV_VERSION >= 0x030000) cv::Mat Ip = cv::imread(filename.c_str(), cv::IMREAD_GRAYSCALE); if (!Ip.empty()) vpImageConvert::convert(Ip, I); else - throw( - vpImageException(vpImageException::ioError, "Can't read the image")); + throw(vpImageException(vpImageException::ioError, "Can't read the image")); #elif (VISP_HAVE_OPENCV_VERSION >= 0x020408) cv::Mat Ip = cv::imread(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE); if (!Ip.empty()) vpImageConvert::convert(Ip, I); else - throw( - vpImageException(vpImageException::ioError, "Can't read the image")); + throw(vpImageException(vpImageException::ioError, "Can't read the image")); #else IplImage *Ip = NULL; Ip = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE); if (Ip != NULL) vpImageConvert::convert(Ip, I); else - throw( - vpImageException(vpImageException::ioError, "Can't read the image")); + throw(vpImageException(vpImageException::ioError, "Can't read the image")); cvReleaseImage(&Ip); #endif } @@ -1430,23 +1336,20 @@ void vpImageIo::readJPEG(vpImage &I, const std::string &filename) if (!Ip.empty()) vpImageConvert::convert(Ip, I); else - throw( - vpImageException(vpImageException::ioError, "Can't read the image")); + throw(vpImageException(vpImageException::ioError, "Can't read the image")); #elif (VISP_HAVE_OPENCV_VERSION >= 0x020408) cv::Mat Ip = cv::imread(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE); if (!Ip.empty()) vpImageConvert::convert(Ip, I); else - throw( - vpImageException(vpImageException::ioError, "Can't read the image")); + throw(vpImageException(vpImageException::ioError, "Can't read the image")); #else IplImage *Ip = NULL; Ip = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR); if (Ip != NULL) vpImageConvert::convert(Ip, I); else - throw( - vpImageException(vpImageException::ioError, "Can't read the image")); + throw(vpImageException(vpImageException::ioError, "Can't read the image")); cvReleaseImage(&Ip); #endif } @@ -1466,28 +1369,23 @@ void vpImageIo::readJPEG(vpImage &I, const std::string &filename) \param I : Image to save as a PNG file. \param filename : Name of the file containing the image. */ -void vpImageIo::writePNG(const vpImage &I, - const std::string &filename) +void vpImageIo::writePNG(const vpImage &I, const std::string &filename) { FILE *file; // Test the filename if (filename.empty()) { - throw(vpImageException(vpImageException::ioError, - "Cannot create PNG file: filename empty")); + throw(vpImageException(vpImageException::ioError, "Cannot create PNG file: filename empty")); } file = fopen(filename.c_str(), "wb"); if (file == NULL) { - throw(vpImageException(vpImageException::ioError, - "Cannot create PNG file \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot create PNG file \"%s\"", filename.c_str())); } /* create a png info struct */ - png_structp png_ptr = - png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL); + 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"); @@ -1528,8 +1426,7 @@ void vpImageIo::writePNG(const vpImage &I, 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_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); @@ -1569,28 +1466,23 @@ void vpImageIo::writePNG(const vpImage &I, \param I : Image to save as a PNG file. \param filename : Name of the file containing the image. */ -void vpImageIo::writePNG(const vpImage &I, - const std::string &filename) +void vpImageIo::writePNG(const vpImage &I, const std::string &filename) { FILE *file; // Test the filename if (filename.empty()) { - throw(vpImageException(vpImageException::ioError, - "Cannot create PNG file: filename empty")); + throw(vpImageException(vpImageException::ioError, "Cannot create PNG file: filename empty")); } file = fopen(filename.c_str(), "wb"); if (file == NULL) { - throw(vpImageException(vpImageException::ioError, - "Cannot create PNG file \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot create PNG file \"%s\"", filename.c_str())); } /* create a png info struct */ - png_structp png_ptr = - png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL); + 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"); @@ -1631,8 +1523,7 @@ void vpImageIo::writePNG(const vpImage &I, 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_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); @@ -1687,50 +1578,41 @@ void vpImageIo::writePNG(const vpImage &I, \param filename : Name of the file containing the image. */ -void vpImageIo::readPNG(vpImage &I, - const std::string &filename) +void vpImageIo::readPNG(vpImage &I, const std::string &filename) { FILE *file; png_byte magic[8]; // Test the filename if (filename.empty()) { - throw(vpImageException(vpImageException::ioError, - "Cannot read PNG image: filename empty")); + throw(vpImageException(vpImageException::ioError, "Cannot read PNG image: filename empty")); } file = fopen(filename.c_str(), "rb"); if (file == NULL) { - throw(vpImageException(vpImageException::ioError, - "Cannot read file \"%s\"", filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot read file \"%s\"", filename.c_str())); } /* read magic number */ if (fread(magic, 1, sizeof(magic), file) != sizeof(magic)) { fclose(file); - throw(vpImageException(vpImageException::ioError, - "Cannot read magic number in file \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot read magic number in file \"%s\"", filename.c_str())); } /* check for valid magic number */ if (png_sig_cmp(magic, 0, sizeof(magic))) { fclose(file); - throw(vpImageException( - vpImageException::ioError, - "Cannot read PNG file: \"%s\" is not a valid PNG image", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot read PNG file: \"%s\" is not a valid PNG image", + filename.c_str())); } /* 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); + 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")); + throw(vpImageException(vpImageException::ioError, "error reading png file")); } /* create a png info struct */ @@ -1739,8 +1621,7 @@ void vpImageIo::readPNG(vpImage &I, 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")); + throw(vpImageException(vpImageException::ioError, "error reading png file")); } /* initialize the setjmp for returning properly after a libpng error occured @@ -1881,37 +1762,30 @@ void vpImageIo::readPNG(vpImage &I, const std::string &filename) // Test the filename if (filename.empty()) { - throw(vpImageException(vpImageException::ioError, - "Cannot read PNG image: filename empty")); + throw(vpImageException(vpImageException::ioError, "Cannot read PNG image: filename empty")); } file = fopen(filename.c_str(), "rb"); if (file == NULL) { - throw(vpImageException(vpImageException::ioError, - "Cannot read file \"%s\"", filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot read file \"%s\"", filename.c_str())); } /* read magic number */ if (fread(magic, 1, sizeof(magic), file) != sizeof(magic)) { fclose(file); - throw(vpImageException(vpImageException::ioError, - "Cannot read magic number in file \"%s\"", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot read magic number in file \"%s\"", filename.c_str())); } /* check for valid magic number */ if (png_sig_cmp(magic, 0, sizeof(magic))) { fclose(file); - throw(vpImageException( - vpImageException::ioError, - "Cannot read PNG file: \"%s\" is not a valid PNG image", - filename.c_str())); + throw(vpImageException(vpImageException::ioError, "Cannot read PNG file: \"%s\" is not a valid PNG image", + filename.c_str())); } /* create a png read struct */ - png_structp png_ptr = - png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL); + 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"); @@ -2049,8 +1923,7 @@ void vpImageIo::readPNG(vpImage &I, const std::string &filename) \param I : Image to save as a PNG file. \param filename : Name of the file containing the image. */ -void vpImageIo::writePNG(const vpImage &I, - const std::string &filename) +void vpImageIo::writePNG(const vpImage &I, const std::string &filename) { #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) cv::Mat Ip; @@ -2073,8 +1946,7 @@ void vpImageIo::writePNG(const vpImage &I, \param I : Image to save as a PNG file. \param filename : Name of the file containing the image. */ -void vpImageIo::writePNG(const vpImage &I, - const std::string &filename) +void vpImageIo::writePNG(const vpImage &I, const std::string &filename) { #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) cv::Mat Ip; @@ -2106,31 +1978,27 @@ void vpImageIo::writePNG(const vpImage &I, \param filename : Name of the file containing the image. */ -void vpImageIo::readPNG(vpImage &I, - const std::string &filename) +void vpImageIo::readPNG(vpImage &I, const std::string &filename) { #if (VISP_HAVE_OPENCV_VERSION >= 0x030000) cv::Mat Ip = cv::imread(filename.c_str(), cv::IMREAD_GRAYSCALE); if (!Ip.empty()) vpImageConvert::convert(Ip, I); else - throw( - vpImageException(vpImageException::ioError, "Can't read the image")); + throw(vpImageException(vpImageException::ioError, "Can't read the image")); #elif (VISP_HAVE_OPENCV_VERSION >= 0x020408) cv::Mat Ip = cv::imread(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE); if (!Ip.empty()) vpImageConvert::convert(Ip, I); else - throw( - vpImageException(vpImageException::ioError, "Can't read the image")); + throw(vpImageException(vpImageException::ioError, "Can't read the image")); #else IplImage *Ip = NULL; Ip = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE); if (Ip != NULL) vpImageConvert::convert(Ip, I); else - throw( - vpImageException(vpImageException::ioError, "Can't read the image")); + throw(vpImageException(vpImageException::ioError, "Can't read the image")); cvReleaseImage(&Ip); #endif } @@ -2160,23 +2028,20 @@ void vpImageIo::readPNG(vpImage &I, const std::string &filename) if (!Ip.empty()) vpImageConvert::convert(Ip, I); else - throw( - vpImageException(vpImageException::ioError, "Can't read the image")); + throw(vpImageException(vpImageException::ioError, "Can't read the image")); #elif (VISP_HAVE_OPENCV_VERSION >= 0x020408) cv::Mat Ip = cv::imread(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE); if (!Ip.empty()) vpImageConvert::convert(Ip, I); else - throw( - vpImageException(vpImageException::ioError, "Can't read the image")); + throw(vpImageException(vpImageException::ioError, "Can't read the image")); #else IplImage *Ip = NULL; Ip = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR); if (Ip != NULL) vpImageConvert::convert(Ip, I); else - throw( - vpImageException(vpImageException::ioError, "Can't read the image")); + throw(vpImageException(vpImageException::ioError, "Can't read the image")); cvReleaseImage(&Ip); #endif } diff --git a/modules/io/src/parallel-port/vpParallelPort.cpp b/modules/io/src/parallel-port/vpParallelPort.cpp index 38b341f98e..5536e1ee54 100644 --- a/modules/io/src/parallel-port/vpParallelPort.cpp +++ b/modules/io/src/parallel-port/vpParallelPort.cpp @@ -102,8 +102,7 @@ void vpParallelPort::open() 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")); + throw(vpParallelPortException(vpParallelPortException::opening, "Can't open /dev/parport0")); } int i; @@ -163,8 +162,7 @@ void vpParallelPort::close() if (err != 0) { printf("Can't close the parallel port\n"); - throw(vpParallelPortException(vpParallelPortException::closing, - "Can't close the parallel port")); + throw(vpParallelPortException(vpParallelPortException::closing, "Can't close the parallel port")); } } diff --git a/modules/io/src/tools/vpKeyboard.cpp b/modules/io/src/tools/vpKeyboard.cpp index 0a960e6eb1..f11ba6dad6 100644 --- a/modules/io/src/tools/vpKeyboard.cpp +++ b/modules/io/src/tools/vpKeyboard.cpp @@ -36,8 +36,7 @@ * *****************************************************************************/ -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) #include #include diff --git a/modules/io/src/tools/vpParseArgv.cpp b/modules/io/src/tools/vpParseArgv.cpp index 1c438c3219..025009c341 100644 --- a/modules/io/src/tools/vpParseArgv.cpp +++ b/modules/io/src/tools/vpParseArgv.cpp @@ -38,13 +38,11 @@ */ vpParseArgv::vpArgvInfo vpParseArgv::defaultTable[2] = { - {"-help", ARGV_HELP, (char *)NULL, (char *)NULL, - "Print summary of command-line options and abort.\n"}, + {"-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); +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. @@ -68,8 +66,7 @@ int (*handlerProc2)(const char *dst, const char *key, int valargc, 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) +bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags) { vpArgvInfo *infoPtr; /* Pointer to the current entry in the @@ -90,8 +87,8 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, unsigned long long nargs; /* Number of following arguments to get. */ /* Macro to optionally print errors */ -#define FPRINTF \ - if (!(flags & ARGV_NO_PRINT)) \ +#define FPRINTF \ + if (!(flags & ARGV_NO_PRINT)) \ (void)fprintf if (flags & ARGV_DONT_SKIP_FIRST_ARG) { @@ -126,8 +123,7 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, if (infoPtr->key == NULL) { continue; } - if ((infoPtr->key[1] != c) || - (strncmp(infoPtr->key, curArg, length) != 0)) { + if ((infoPtr->key[1] != c) || (strncmp(infoPtr->key, curArg, length) != 0)) { continue; } if (infoPtr->key[length] == 0) { @@ -182,12 +178,9 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, } else { char *endPtr = NULL; - *(((int *)infoPtr->dst) + i) = - (int)strtol(argv[srcIndex], &endPtr, 0); + *(((int *)infoPtr->dst) + i) = (int)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]); + FPRINTF(stderr, "expected integer argument for \"%s\" but got \"%s\"\n", infoPtr->key, argv[srcIndex]); return true; } srcIndex++; @@ -207,9 +200,7 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, *(((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]); + FPRINTF(stderr, "expected long argument for \"%s\" but got \"%s\"\n", infoPtr->key, argv[srcIndex]); return true; } srcIndex++; @@ -244,13 +235,10 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, } else { char *endPtr; - *(((float *)infoPtr->dst) + i) = - (float)strtod(argv[srcIndex], &endPtr); // Here we use strtod + *(((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]); + FPRINTF(stderr, "expected floating-point argument for \"%s\" but got\"%s\"\n", infoPtr->key, + argv[srcIndex]); return true; } srcIndex++; @@ -270,10 +258,7 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, *(((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]); + FPRINTF(stderr, "expected double-point argument for \"%s\" but got\"%s\"\n", infoPtr->key, argv[srcIndex]); return true; } srcIndex++; @@ -283,8 +268,7 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, break; case ARGV_FUNC: { - handlerProc1 = (int (*)(const char *dst, const char *key, - const char *argument))infoPtr->src; + handlerProc1 = (int (*)(const char *dst, const char *key, const char *argument))infoPtr->src; if ((*handlerProc1)(infoPtr->dst, infoPtr->key, argv[srcIndex])) { srcIndex += 1; @@ -293,11 +277,9 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, break; } case ARGV_GENFUNC: { - handlerProc2 = (int (*)(const char *dst, const char *key, int valargc, - const char **argument))infoPtr->src; + handlerProc2 = (int (*)(const char *dst, const char *key, int valargc, const char **argument))infoPtr->src; - argc = - (*handlerProc2)(infoPtr->dst, infoPtr->key, argc, argv + srcIndex); + argc = (*handlerProc2)(infoPtr->dst, infoPtr->key, argc, argv + srcIndex); if (argc < 0) { return true; } @@ -362,8 +344,8 @@ void vpParseArgv::printUsage(vpArgvInfo *argTable, int flags) unsigned long long nargs; /* Macro to optionally print errors */ -#define FPRINTF \ - if (!(flags & ARGV_NO_PRINT)) \ +#define FPRINTF \ + if (!(flags & ARGV_NO_PRINT)) \ (void)fprintf /* @@ -373,8 +355,7 @@ void vpParseArgv::printUsage(vpArgvInfo *argTable, int flags) width = 4; for (unsigned int i = 0; i < 2; i++) { - for (infoPtr = i ? defaultTable : argTable; infoPtr->type != ARGV_END; - infoPtr++) { + for (infoPtr = i ? defaultTable : argTable; infoPtr->type != ARGV_END; infoPtr++) { int length; if (infoPtr->key == NULL) { continue; @@ -388,8 +369,7 @@ void vpParseArgv::printUsage(vpArgvInfo *argTable, int flags) FPRINTF(stderr, "Command-specific options:"); for (unsigned int i = 0;; i++) { - for (infoPtr = i ? defaultTable : argTable; infoPtr->type != ARGV_END; - infoPtr++) { + 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; @@ -515,8 +495,7 @@ void vpParseArgv::printUsage(vpArgvInfo *argTable, int flags) is NULL. */ -int vpParseArgv::parse(int argc, const char **argv, const char *validOpts, - const char **param) +int vpParseArgv::parse(int argc, const char **argv, const char *validOpts, const char **param) { static int iArg = 1; int chOpt; diff --git a/modules/io/src/video/vpDiskGrabber.cpp b/modules/io/src/video/vpDiskGrabber.cpp index be56172055..ca2d7bbfef 100644 --- a/modules/io/src/video/vpDiskGrabber.cpp +++ b/modules/io/src/video/vpDiskGrabber.cpp @@ -42,9 +42,8 @@ Elementary constructor. */ vpDiskGrabber::vpDiskGrabber() - : m_image_number(0), m_image_number_next(0), m_image_step(1), - m_number_of_zero(0), m_directory("/tmp"), m_base_name("I"), - m_extension("pgm"), m_use_generic_name(false), m_generic_name("empty") + : m_image_number(0), m_image_number_next(0), m_image_step(1), m_number_of_zero(0), m_directory("/tmp"), + m_base_name("I"), m_extension("pgm"), m_use_generic_name(false), m_generic_name("empty") { init = false; } @@ -53,9 +52,8 @@ vpDiskGrabber::vpDiskGrabber() Constructor that takes a generic image sequence as input. */ vpDiskGrabber::vpDiskGrabber(const std::string &generic_name) - : m_image_number(0), m_image_number_next(0), m_image_step(1), - m_number_of_zero(0), m_directory("/tmp"), m_base_name("I"), - m_extension("pgm"), m_use_generic_name(true), m_generic_name(generic_name) + : m_image_number(0), m_image_number_next(0), m_image_step(1), m_number_of_zero(0), m_directory("/tmp"), + m_base_name("I"), m_extension("pgm"), m_use_generic_name(true), m_generic_name(generic_name) { init = false; } @@ -71,13 +69,10 @@ vpDiskGrabber::vpDiskGrabber(const std::string &generic_name) \param ext : Extension of the image file. */ -vpDiskGrabber::vpDiskGrabber(const std::string &dir, - const std::string &basename, long number, - int step, unsigned int noz, - const std::string &ext) - : m_image_number(number), m_image_number_next(number), m_image_step(step), - m_number_of_zero(noz), m_directory(dir), m_base_name(basename), - m_extension(ext), m_use_generic_name(false), m_generic_name("empty") +vpDiskGrabber::vpDiskGrabber(const std::string &dir, const std::string &basename, long number, int step, + unsigned int noz, const std::string &ext) + : m_image_number(number), m_image_number_next(number), m_image_step(step), m_number_of_zero(noz), m_directory(dir), + m_base_name(basename), m_extension(ext), m_use_generic_name(false), m_generic_name("empty") { init = false; } @@ -156,8 +151,8 @@ void vpDiskGrabber::acquire(vpImage &I) sprintf(filename, m_generic_name.c_str(), m_image_number); ss << filename; } else { - ss << m_directory << "/" << m_base_name << std::setfill('0') - << std::setw(m_number_of_zero) << m_image_number << "." << m_extension; + ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << m_image_number << "." + << m_extension; } m_image_number_next += m_image_step; @@ -183,8 +178,8 @@ void vpDiskGrabber::acquire(vpImage &I) sprintf(filename, m_generic_name.c_str(), m_image_number); ss << filename; } else { - ss << m_directory << "/" << m_base_name << std::setfill('0') - << std::setw(m_number_of_zero) << m_image_number << "." << m_extension; + ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << m_image_number << "." + << m_extension; } m_image_number_next += m_image_step; @@ -210,8 +205,8 @@ void vpDiskGrabber::acquire(vpImage &I) sprintf(filename, m_generic_name.c_str(), m_image_number); ss << filename; } else { - ss << m_directory << "/" << m_base_name << std::setfill('0') - << std::setw(m_number_of_zero) << m_image_number << "." << m_extension; + ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << m_image_number << "." + << m_extension; } m_image_number_next += m_image_step; @@ -238,8 +233,8 @@ void vpDiskGrabber::acquire(vpImage &I, long img_number) sprintf(filename, m_generic_name.c_str(), m_image_number); ss << filename; } else { - ss << m_directory << "/" << m_base_name << std::setfill('0') - << std::setw(m_number_of_zero) << img_number << "." << m_extension; + ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << img_number << "." + << m_extension; } m_image_number_next += m_image_step; @@ -266,8 +261,8 @@ void vpDiskGrabber::acquire(vpImage &I, long img_number) sprintf(filename, m_generic_name.c_str(), m_image_number); ss << filename; } else { - ss << m_directory << "/" << m_base_name << std::setfill('0') - << std::setw(m_number_of_zero) << img_number << "." << m_extension; + ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << img_number << "." + << m_extension; } m_image_number_next += m_image_step; @@ -294,8 +289,8 @@ void vpDiskGrabber::acquire(vpImage &I, long img_number) sprintf(filename, m_generic_name.c_str(), m_image_number); ss << filename; } else { - ss << m_directory << "/" << m_base_name << std::setfill('0') - << std::setw(m_number_of_zero) << img_number << "." << m_extension; + ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << img_number << "." + << m_extension; } m_image_number_next += m_image_step; @@ -326,26 +321,17 @@ vpDiskGrabber::~vpDiskGrabber() {} /*! Set the main directory name (ie location of the image sequence) */ -void vpDiskGrabber::setDirectory(const std::string &dir) -{ - m_directory = dir; -} +void vpDiskGrabber::setDirectory(const std::string &dir) { m_directory = dir; } /*! Set the image base name. */ -void vpDiskGrabber::setBaseName(const std::string &name) -{ - m_base_name = name; -} +void vpDiskGrabber::setBaseName(const std::string &name) { m_base_name = name; } /*! Set the image extension. */ -void vpDiskGrabber::setExtension(const std::string &ext) -{ - m_extension = ext; -} +void vpDiskGrabber::setExtension(const std::string &ext) { m_extension = ext; } /*! Set the number of the image to be read. @@ -363,10 +349,7 @@ void vpDiskGrabber::setStep(long step) { m_image_step = step; } /*! Set the step between two images. */ -void vpDiskGrabber::setNumberOfZero(unsigned int noz) -{ - m_number_of_zero = noz; -} +void vpDiskGrabber::setNumberOfZero(unsigned int noz) { m_number_of_zero = noz; } void vpDiskGrabber::setGenericName(const std::string &generic_name) { diff --git a/modules/io/src/video/vpVideoReader.cpp b/modules/io/src/video/vpVideoReader.cpp index f72a4e3677..37da578f6a 100644 --- a/modules/io/src/video/vpVideoReader.cpp +++ b/modules/io/src/video/vpVideoReader.cpp @@ -59,9 +59,8 @@ vpVideoReader::vpVideoReader() #if 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), frameStep(1), frameRate(0.) + formatType(FORMAT_UNKNOWN), initFileName(false), isOpen(false), frameCount(0), firstFrame(0), lastFrame(0), + firstFrameIndexIsSet(false), lastFrameIndexIsSet(false), frameStep(1), frameRate(0.) { } @@ -94,13 +93,11 @@ void vpVideoReader::setFileName(const char *filename) { if ((!filename) || (*filename == '\0')) { vpERROR_TRACE("filename empty "); - throw(vpImageException(vpImageException::noFileNameError, - "filename empty ")); + throw(vpImageException(vpImageException::noFileNameError, "filename empty ")); } if (strlen(filename) >= FILENAME_MAX) { - throw(vpException(vpException::memoryAllocationError, - "Not enough memory to initialize the file name")); + throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the file name")); } strcpy(this->fileName, filename); @@ -108,17 +105,14 @@ void vpVideoReader::setFileName(const char *filename) formatType = getFormat(fileName); if (formatType == FORMAT_UNKNOWN) { - throw(vpException(vpException::badValue, - "Filename extension not supported")); + throw(vpException(vpException::badValue, "Filename extension not supported")); } // checking image name format if (isImageExtensionSupported()) { std::string format = vpIoTools::getName(fileName); if (!checkImageNameFormat(format)) { - throw(vpException(vpException::badValue, - "Format of image name wasn't recognized: %s", - format.c_str())); + throw(vpException(vpException::badValue, "Format of image name wasn't recognized: %s", format.c_str())); } } @@ -140,10 +134,7 @@ 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()); -} +void vpVideoReader::setFileName(const std::string &filename) { setFileName(filename.c_str()); } /*! Open video stream and get first and last frame indexes. @@ -151,8 +142,7 @@ void vpVideoReader::setFileName(const std::string &filename) void vpVideoReader::getProperties() { if (!initFileName) { - throw(vpImageException(vpImageException::noFileNameError, - "The generic filename has to be set")); + throw(vpImageException(vpImageException::noFileNameError, "The generic filename has to be set")); } if (isImageExtensionSupported()) { @@ -168,8 +158,7 @@ void vpVideoReader::getProperties() capture.open(fileName); if (!capture.isOpened()) { - throw(vpException(vpException::ioError, - "Could not open the video %s with OpenCV", fileName)); + throw(vpException(vpException::ioError, "Could not open the video %s with OpenCV", fileName)); } #if VISP_HAVE_OPENCV_VERSION >= 0x030000 width = (unsigned int)capture.get(cv::CAP_PROP_FRAME_WIDTH); @@ -182,9 +171,8 @@ void vpVideoReader::getProperties() #endif #else - throw(vpException(vpException::fatalError, - "To read video files ViSP should be build with opencv " - "3rd >= 2.1.0 party libraries.")); + throw(vpException(vpException::fatalError, "To read video files ViSP should be build with 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 @@ -212,8 +200,7 @@ void vpVideoReader::open(vpImage &I) frameCount = firstFrame; if (!getFrame(I, firstFrame)) { - throw(vpException(vpException::ioError, - "Could not read the video first frame")); + throw(vpException(vpException::ioError, "Could not read the video first frame")); } // Rewind to the first frame since open() should not increase the frame @@ -246,8 +233,7 @@ void vpVideoReader::open(vpImage &I) frameCount = firstFrame; if (!getFrame(I, firstFrame)) { - throw(vpException(vpException::ioError, - "Could not read the video first frame")); + throw(vpException(vpException::ioError, "Could not read the video first frame")); } // Rewind to the first frame since open() should not increase the frame @@ -433,8 +419,7 @@ bool vpVideoReader::getFrame(vpImage &I, long frame_index) width = I.getWidth(); height = I.getHeight(); frameCount = imSequence->getImageNumber(); - imSequence->setImageNumber( - frameCount); // to not increment vpDiskGrabber next image + imSequence->setImageNumber(frameCount); // to not increment vpDiskGrabber next image if (frameCount + frameStep > lastFrame) { imSequence->setImageNumber(frameCount); } else if (frameCount + frameStep < firstFrame) { @@ -504,8 +489,7 @@ bool vpVideoReader::getFrame(vpImage &I, long frame_index) width = I.getWidth(); height = I.getHeight(); frameCount = imSequence->getImageNumber(); - imSequence->setImageNumber( - frameCount); // to not increment vpDiskGrabber next image + imSequence->setImageNumber(frameCount); // to not increment vpDiskGrabber next image if (frameCount + frameStep > lastFrame) { imSequence->setImageNumber(frameCount); } else if (frameCount + frameStep < firstFrame) { @@ -563,8 +547,7 @@ Gets the format of the file(s) which has/have to be read. \return Returns the format. */ -vpVideoReader::vpVideoFormatType -vpVideoReader::getFormat(const char *filename) +vpVideoReader::vpVideoFormatType vpVideoReader::getFormat(const char *filename) { std::string sfilename(filename); @@ -744,8 +727,7 @@ void vpVideoReader::findFirstFrameIndex() // Checking that file name satisfies image format, specified by // imageNameFormat, and extracting imageIndex long imageIndex = extractImageIndex(files[i], imageNameFormat); - if ((imageIndex != -1) && - (imageIndex < firstFrame || firstFrame == -1)) { + if ((imageIndex != -1) && (imageIndex < firstFrame || firstFrame == -1)) { firstFrame = imageIndex; } } @@ -764,11 +746,10 @@ 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 (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); } /*! @@ -776,9 +757,8 @@ 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 || + 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); } @@ -853,8 +833,7 @@ vpVideoReader &vpVideoReader::operator>>(vpImage &I) \param format : format of image name \return extracted index on success, -1 otherwise. */ -long vpVideoReader::extractImageIndex(const std::string &imageName, - const std::string &format) +long vpVideoReader::extractImageIndex(const std::string &imageName, const std::string &format) { size_t indexBegin = format.find_last_of('%'); size_t indexEnd = format.find_first_of('d', indexBegin); diff --git a/modules/io/src/video/vpVideoWriter.cpp b/modules/io/src/video/vpVideoWriter.cpp index 01fdbc7732..e081b337e3 100644 --- a/modules/io/src/video/vpVideoWriter.cpp +++ b/modules/io/src/video/vpVideoWriter.cpp @@ -57,8 +57,7 @@ vpVideoWriter::vpVideoWriter() #if 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) + formatType(FORMAT_UNKNOWN), initFileName(false), isOpen(false), frameCount(0), firstFrame(0), width(0), height(0) { initFileName = false; firstFrame = 0; @@ -94,13 +93,11 @@ void vpVideoWriter::setFileName(const char *filename) { if (!filename || *filename == '\0') { vpERROR_TRACE("filename empty "); - throw(vpImageException(vpImageException::noFileNameError, - "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")); + throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the file name")); } strcpy(this->fileName, filename); @@ -108,8 +105,7 @@ void vpVideoWriter::setFileName(const char *filename) formatType = getFormat(fileName); if (formatType == FORMAT_UNKNOWN) { - throw(vpException(vpException::badValue, - "Filename extension not supported")); + throw(vpException(vpException::badValue, "Filename extension not supported")); } initFileName = true; @@ -126,10 +122,7 @@ void vpVideoWriter::setFileName(const char *filename) \param filename : filename template of an image sequence. */ -void vpVideoWriter::setFileName(const std::string &filename) -{ - setFileName(filename.c_str()); -} +void vpVideoWriter::setFileName(const std::string &filename) { setFileName(filename.c_str()); } /*! Sets all the parameters needed to write the video or the image sequence. @@ -140,29 +133,24 @@ void vpVideoWriter::open(vpImage &I) { if (!initFileName) { vpERROR_TRACE("The generic filename has to be set"); - throw(vpImageException(vpImageException::noFileNameError, - "filename empty")); + throw(vpImageException(vpImageException::noFileNameError, "filename empty")); } - if (formatType == FORMAT_PGM || formatType == FORMAT_PPM || - formatType == FORMAT_JPEG || formatType == FORMAT_PNG) { + 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) { + } else if (formatType == FORMAT_AVI || formatType == FORMAT_MPEG || formatType == FORMAT_MPEG4 || + formatType == FORMAT_MOV) { #if VISP_HAVE_OPENCV_VERSION >= 0x020100 - writer = cv::VideoWriter(fileName, fourcc, framerate, - cv::Size((int)I.getWidth(), (int)I.getHeight())); + 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")); + throw(vpException(vpException::fatalError, "Could not open encode the video with opencv")); } #else - throw(vpException(vpException::fatalError, - "To encode video files ViSP should be build with " - "opencv 3rd >= 2.1.0 party libraries.")); + throw(vpException(vpException::fatalError, "To encode video files ViSP should be build with " + "opencv 3rd >= 2.1.0 party libraries.")); #endif } @@ -180,29 +168,24 @@ void vpVideoWriter::open(vpImage &I) { if (!initFileName) { vpERROR_TRACE("The generic filename has to be set"); - throw(vpImageException(vpImageException::noFileNameError, - "filename empty")); + throw(vpImageException(vpImageException::noFileNameError, "filename empty")); } - if (formatType == FORMAT_PGM || formatType == FORMAT_PPM || - formatType == FORMAT_JPEG || formatType == FORMAT_PNG) { + 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) { + } else if (formatType == FORMAT_AVI || formatType == FORMAT_MPEG || formatType == FORMAT_MPEG4 || + formatType == FORMAT_MOV) { #if VISP_HAVE_OPENCV_VERSION >= 0x020100 - writer = cv::VideoWriter(fileName, fourcc, framerate, - cv::Size((int)I.getWidth(), (int)I.getHeight())); + 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")); + throw(vpException(vpException::ioError, "Could not encode the video with opencv")); } #else - throw(vpException(vpException::fatalError, - "To encode video files ViSP should be build with " - "opencv 3rd >= 2.1.0 party libraries.")); + throw(vpException(vpException::fatalError, "To encode video files ViSP should be build with " + "opencv 3rd >= 2.1.0 party libraries.")); #endif } @@ -227,8 +210,7 @@ void vpVideoWriter::saveFrame(vpImage &I) throw(vpException(vpException::notInitialized, "file not yet opened")); } - if (formatType == FORMAT_PGM || formatType == FORMAT_PPM || - formatType == FORMAT_JPEG || formatType == FORMAT_PNG) { + if (formatType == FORMAT_PGM || formatType == FORMAT_PPM || formatType == FORMAT_JPEG || formatType == FORMAT_PNG) { char name[FILENAME_MAX]; sprintf(name, fileName, frameCount); @@ -261,8 +243,7 @@ void vpVideoWriter::saveFrame(vpImage &I) throw(vpException(vpException::notInitialized, "file not yet opened")); } - if (formatType == FORMAT_PGM || formatType == FORMAT_PPM || - formatType == FORMAT_JPEG || formatType == FORMAT_PNG) { + if (formatType == FORMAT_PGM || formatType == FORMAT_PPM || formatType == FORMAT_JPEG || formatType == FORMAT_PNG) { char name[FILENAME_MAX]; sprintf(name, fileName, frameCount); @@ -301,8 +282,7 @@ void vpVideoWriter::close() \return Returns the format. */ -vpVideoWriter::vpVideoFormatType -vpVideoWriter::getFormat(const char *filename) +vpVideoWriter::vpVideoFormatType vpVideoWriter::getFormat(const char *filename) { std::string sfilename(filename); diff --git a/modules/robot/include/visp3/robot/vpAfma4.h b/modules/robot/include/visp3/robot/vpAfma4.h index 1b431c44b3..276d4ec3df 100644 --- a/modules/robot/include/visp3/robot/vpAfma4.h +++ b/modules/robot/include/visp3/robot/vpAfma4.h @@ -132,8 +132,7 @@ class VISP_EXPORT vpAfma4 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); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma4 &afma4); vpColVector getJointMin() const; vpColVector getJointMax() const; diff --git a/modules/robot/include/visp3/robot/vpAfma6.h b/modules/robot/include/visp3/robot/vpAfma6.h index f541a07dfa..733ee88036 100644 --- a/modules/robot/include/visp3/robot/vpAfma6.h +++ b/modules/robot/include/visp3/robot/vpAfma6.h @@ -135,17 +135,15 @@ class VISP_EXPORT vpAfma6 //@{ void init(void); void init(const std::string &camera_extrinsic_parameters); - void init(const std::string &camera_extrinsic_parameters, - const std::string &camera_intrinsic_parameters); + void init(const std::string &camera_extrinsic_parameters, const std::string &camera_intrinsic_parameters); void init(vpAfma6::vpAfma6ToolType tool, const std::string &filename); void init(vpAfma6::vpAfma6ToolType tool, const vpHomogeneousMatrix &eMc_); - void init(vpAfma6::vpAfma6ToolType tool, - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithoutDistortion); + 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, + int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest = true, const bool &verbose = false) const; vpHomogeneousMatrix get_eMc() const; @@ -161,19 +159,12 @@ class VISP_EXPORT vpAfma6 //! 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, + 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 &I) const; - void getCameraParameters(vpCameraParameters &cam, - const vpImage &I) const; + void getCameraParameters(vpCameraParameters &cam, const vpImage &I) const; + void getCameraParameters(vpCameraParameters &cam, const vpImage &I) const; vpColVector getJointMin() const; vpColVector getJointMax() const; @@ -185,8 +176,7 @@ class VISP_EXPORT vpAfma6 virtual void set_eMc(const vpHomogeneousMatrix &eMc); //@} - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpAfma6 &afma6); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6); protected: /** @name Protected Member Functions Inherited from vpAfma6 */ diff --git a/modules/robot/include/visp3/robot/vpBiclops.h b/modules/robot/include/visp3/robot/vpBiclops.h index d771eab1d8..54c8e0285c 100644 --- a/modules/robot/include/visp3/robot/vpBiclops.h +++ b/modules/robot/include/visp3/robot/vpBiclops.h @@ -169,10 +169,7 @@ class VISP_EXPORT vpBiclops Return the Denavit Hartenberg representation used to model the head. \sa vpBiclops::DenavitHartenbergModel */ - inline vpBiclops::DenavitHartenbergModel getDenavitHartenbergModel() const - { - return dh_model_; - } + inline vpBiclops::DenavitHartenbergModel getDenavitHartenbergModel() const { return dh_model_; } void set_cMe(); /*! @@ -185,15 +182,10 @@ class VISP_EXPORT vpBiclops \sa vpBiclops::DenavitHartenbergModel */ - inline void setDenavitHartenbergModel( - vpBiclops::DenavitHartenbergModel m = vpBiclops::DH1) - { - dh_model_ = m; - } + inline void setDenavitHartenbergModel(vpBiclops::DenavitHartenbergModel m = vpBiclops::DH1) { dh_model_ = m; } //@} - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpBiclops &constant); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpBiclops &constant); }; #endif diff --git a/modules/robot/include/visp3/robot/vpImageSimulator.h b/modules/robot/include/visp3/robot/vpImageSimulator.h index 172fa2ec47..5e1e1f9278 100644 --- a/modules/robot/include/visp3/robot/vpImageSimulator.h +++ b/modules/robot/include/visp3/robot/vpImageSimulator.h @@ -228,34 +228,23 @@ class VISP_EXPORT vpImageSimulator // projection du plan sur plan image (coord en metre) void setCameraPosition(const vpHomogeneousMatrix &cMt); - void setInterpolationType(const vpInterpolationType interplt) - { - this->interp = interplt; - } + void setInterpolationType(const vpInterpolationType interplt) { this->interp = interplt; } void getImage(vpImage &I, const vpCameraParameters &cam); void getImage(vpImage &I, const vpCameraParameters &cam); - void getImage(vpImage &I, vpImage &Isrc, - const vpCameraParameters &cam); - void getImage(vpImage &I, vpImage &Isrc, - const vpCameraParameters &cam); + void getImage(vpImage &I, vpImage &Isrc, const vpCameraParameters &cam); + void getImage(vpImage &I, vpImage &Isrc, const vpCameraParameters &cam); - void getImage(vpImage &I, const vpCameraParameters &cam, - vpMatrix &zBuffer); - void getImage(vpImage &I, const vpCameraParameters &cam, - vpMatrix &zBuffer); + void getImage(vpImage &I, const vpCameraParameters &cam, vpMatrix &zBuffer); + void getImage(vpImage &I, const vpCameraParameters &cam, vpMatrix &zBuffer); - static void getImage(vpImage &I, - std::list &list, - const vpCameraParameters &cam); - static void getImage(vpImage &I, std::list &list, - const vpCameraParameters &cam); + static void getImage(vpImage &I, std::list &list, const vpCameraParameters &cam); + static void getImage(vpImage &I, std::list &list, const vpCameraParameters &cam); std::vector get3DcornersTextureRectangle(); - friend VISP_EXPORT std::ostream & - operator<<(std::ostream &os, const vpImageSimulator & /*ip*/); + 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 @@ -268,8 +257,7 @@ class VISP_EXPORT vpImageSimulator \param clean : Enable the reset method. \param color : Color of the back ground. */ - void setCleanPreviousImage(const bool &clean, - const vpColor &color = vpColor::white) + void setCleanPreviousImage(const bool &clean, const vpColor &color = vpColor::white) { cleanPrevImage = clean; bgColor = color; @@ -300,23 +288,19 @@ class VISP_EXPORT vpImageSimulator // 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 &Isrc, const vpImagePoint &iP, - unsigned char &Ipixelplan); - bool getPixel(vpImage &Isrc, const vpImagePoint &iP, - vpRGBa &Ipixelplan); + bool getPixel(vpImage &Isrc, const vpImagePoint &iP, unsigned char &Ipixelplan); + bool getPixel(vpImage &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); + 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, + void getRoi(const unsigned int &Iwidth, const unsigned int &Iheight, const vpCameraParameters &cam, const std::vector &point, vpRect &rect); }; diff --git a/modules/robot/include/visp3/robot/vpPioneer.h b/modules/robot/include/visp3/robot/vpPioneer.h index 021ac2845f..631e7c557a 100644 --- a/modules/robot/include/visp3/robot/vpPioneer.h +++ b/modules/robot/include/visp3/robot/vpPioneer.h @@ -113,8 +113,7 @@ class VISP_EXPORT vpPioneer : public vpUnicycle 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 + 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); diff --git a/modules/robot/include/visp3/robot/vpPtu46.h b/modules/robot/include/visp3/robot/vpPtu46.h index 872aadea49..d98361f63e 100644 --- a/modules/robot/include/visp3/robot/vpPtu46.h +++ b/modules/robot/include/visp3/robot/vpPtu46.h @@ -102,8 +102,7 @@ class VISP_EXPORT vpPtu46 void get_fJe(const vpColVector &q, vpMatrix &fJe) const; //@} - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpPtu46 &constant); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPtu46 &constant); }; #endif diff --git a/modules/robot/include/visp3/robot/vpRingLight.h b/modules/robot/include/visp3/robot/vpRingLight.h index 769cb15f05..7ede0b04ec 100644 --- a/modules/robot/include/visp3/robot/vpRingLight.h +++ b/modules/robot/include/visp3/robot/vpRingLight.h @@ -72,8 +72,8 @@ int main() { #if defined(VISP_HAVE_PARPORT) && defined(VISP_HAVE_DC1394) - vp1394TwoGrabber g; // Firewire framegrabber based on libdc1394-2.x third -party lib vpImage I; + vp1394TwoGrabber g; // Firewire framegrabber based on libdc1394-2.x third party lib + vpImage I; vpRingLight light; // Open the device to access to the ring light. diff --git a/modules/robot/include/visp3/robot/vpRobot.h b/modules/robot/include/visp3/robot/vpRobot.h index 69359f6c31..f89860104c 100644 --- a/modules/robot/include/visp3/robot/vpRobot.h +++ b/modules/robot/include/visp3/robot/vpRobot.h @@ -62,10 +62,10 @@ class VISP_EXPORT vpRobot 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_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; @@ -127,14 +127,12 @@ class VISP_EXPORT vpRobot //! Get a displacement (frame as to ve specified) between two successive //! position control. - virtual void getDisplacement(const vpRobot::vpControlFrameType frame, - vpColVector &q) = 0; + 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; + 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); @@ -147,24 +145,19 @@ class VISP_EXPORT vpRobot 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); + 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; + virtual void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) = 0; inline void setVerbose(bool verbose) { verbose_ = verbose; }; //@} /** @name Static Public Member Functions inherited from vpRobot */ //@{ - static vpColVector saturateVelocities(const vpColVector &v_in, - const vpColVector &v_max, - bool verbose = false); + static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose = false); //@} protected: diff --git a/modules/robot/include/visp3/robot/vpRobotAfma4.h b/modules/robot/include/visp3/robot/vpRobotAfma4.h index 60afbcec33..5f6ea9ec46 100644 --- a/modules/robot/include/visp3/robot/vpRobotAfma4.h +++ b/modules/robot/include/visp3/robot/vpRobotAfma4.h @@ -220,26 +220,20 @@ class VISP_EXPORT vpRobotAfma4 : public vpAfma4, public vpRobot explicit 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); + 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); + 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); + vpColVector getVelocity(const vpRobot::vpControlFrameType frame, double ×tamp); void get_cMe(vpHomogeneousMatrix &cMe) const; void get_cVe(vpVelocityTwistMatrix &cVe) const; @@ -258,10 +252,9 @@ class VISP_EXPORT vpRobotAfma4 : public vpAfma4, public vpRobot static bool savePosFile(const std::string &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 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); @@ -271,8 +264,7 @@ class VISP_EXPORT vpRobotAfma4 : public vpAfma4, public vpRobot /* --- VITESSE ---------------------------------------------------------- */ - void setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &velocity); + void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity); void stopMotion(); }; diff --git a/modules/robot/include/visp3/robot/vpRobotAfma6.h b/modules/robot/include/visp3/robot/vpRobotAfma6.h index 895fdeaaeb..7d56b27615 100644 --- a/modules/robot/include/visp3/robot/vpRobotAfma6.h +++ b/modules/robot/include/visp3/robot/vpRobotAfma6.h @@ -122,8 +122,8 @@ int main() {} \code // Set the extrinsic camera parameters obtained with a perpective // projection model including a distortion parameter - robot.init(vpAfma6::TOOL_CCMOP, -vpCameraParameters::perspectiveProjWithDistortion); \endcode + robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion); + \endcode You can get the intrinsic camera parameters of the image I acquired with the camera, with: @@ -258,30 +258,22 @@ class VISP_EXPORT vpRobotAfma6 : public vpAfma6, public vpRobot void closeGripper(); - void getDisplacement(vpRobot::vpControlFrameType frame, - vpColVector &displacement); + 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); + 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); + 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); + vpColVector getVelocity(const vpRobot::vpControlFrameType frame, double ×tamp); void get_cMe(vpHomogeneousMatrix &_cMe) const; void get_cVe(vpVelocityTwistMatrix &_cVe) const; @@ -291,9 +283,9 @@ class VISP_EXPORT vpRobotAfma6 : public vpAfma6, public vpRobot void init(void); void init(vpAfma6::vpAfma6ToolType tool, const vpHomogeneousMatrix &eMc); void init(vpAfma6::vpAfma6ToolType tool, const std::string &filename); - void init(vpAfma6::vpAfma6ToolType tool, - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithoutDistortion); + void + init(vpAfma6::vpAfma6ToolType tool, + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithoutDistortion); void move(const std::string &filename); void move(const std::string &filename, const double velocity); @@ -307,13 +299,10 @@ class VISP_EXPORT vpRobotAfma6 : public vpAfma6, public vpRobot static bool savePosFile(const std::string &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 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 std::string &filename); void setPositioningVelocity(const double velocity); void set_eMc(const vpHomogeneousMatrix &eMc); @@ -324,8 +313,7 @@ class VISP_EXPORT vpRobotAfma6 : public vpAfma6, public vpRobot /* --- VITESSE ---------------------------------------------------------- */ - void setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &velocity); + void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity); void stopMotion(); }; diff --git a/modules/robot/include/visp3/robot/vpRobotBiclops.h b/modules/robot/include/visp3/robot/vpRobotBiclops.h index 52a90f4353..8b279fe90b 100644 --- a/modules/robot/include/visp3/robot/vpRobotBiclops.h +++ b/modules/robot/include/visp3/robot/vpRobotBiclops.h @@ -135,28 +135,21 @@ class VISP_EXPORT vpRobotBiclops : public vpBiclops, public vpRobot void get_eJe(vpMatrix &_eJe); void get_fJe(vpMatrix &_fJe); - void getDisplacement(const vpRobot::vpControlFrameType frame, - vpColVector &d); + 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); + void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot); vpColVector getVelocity(const vpRobot::vpControlFrameType frame); bool readPositionFile(const std::string &filename, vpColVector &q); - void setConfigFile( - const std::string &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 setConfigFile(const std::string &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); + vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState); + void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot); void stopMotion(); diff --git a/modules/robot/include/visp3/robot/vpRobotCamera.h b/modules/robot/include/visp3/robot/vpRobotCamera.h index 4e15570345..83c14303bd 100644 --- a/modules/robot/include/visp3/robot/vpRobotCamera.h +++ b/modules/robot/include/visp3/robot/vpRobotCamera.h @@ -86,18 +86,15 @@ int main() 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; + 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; + 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 + 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; @@ -128,8 +125,7 @@ class VISP_EXPORT vpRobotCamera : public vpRobotSimulator void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q); void setPosition(const vpHomogeneousMatrix &cMw); - void setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &v); + void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v); //@} private: @@ -137,10 +133,8 @@ class VISP_EXPORT vpRobotCamera : public vpRobotSimulator // 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 */){}; + 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/vpRobotException.h b/modules/robot/include/visp3/robot/vpRobotException.h index 8751da26b6..b1240a098a 100644 --- a/modules/robot/include/visp3/robot/vpRobotException.h +++ b/modules/robot/include/visp3/robot/vpRobotException.h @@ -132,10 +132,7 @@ class VISP_EXPORT vpRobotException : public vpException setMessage(format, args); va_end(args); } - vpRobotException(const int id, const std::string &msg) - : vpException(id, msg) - { - } + vpRobotException(const int id, const std::string &msg) : vpException(id, msg) {} explicit vpRobotException(const int id) : vpException(id) {} }; diff --git a/modules/robot/include/visp3/robot/vpRobotPioneer.h b/modules/robot/include/visp3/robot/vpRobotPioneer.h index deededa774..8ab6945c53 100644 --- a/modules/robot/include/visp3/robot/vpRobotPioneer.h +++ b/modules/robot/include/visp3/robot/vpRobotPioneer.h @@ -66,9 +66,7 @@ about the model of the robot, see vpPioneer documentation. */ -class VISP_EXPORT vpRobotPioneer : public vpRobot, - public vpPioneer, - public ArRobot +class VISP_EXPORT vpRobotPioneer : public vpRobot, public vpPioneer, public ArRobot { private: /* Not allowed functions. */ /*! @@ -105,12 +103,10 @@ class VISP_EXPORT vpRobotPioneer : public vpRobot, Get a displacement (frame as to ve specified) between two successive position control. \warning Not implemented. */ - void getDisplacement(const vpRobot::vpControlFrameType /*frame*/, - vpColVector & /*q*/){}; + void getDisplacement(const vpRobot::vpControlFrameType /*frame*/, vpColVector & /*q*/){}; public: - void getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &velocity); + void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity); vpColVector getVelocity(const vpRobot::vpControlFrameType frame); private: // Set as private since not implemented @@ -118,8 +114,7 @@ class VISP_EXPORT vpRobotPioneer : public vpRobot, Get the robot position (frame has to be specified). \warning Not implemented. */ - void getPosition(const vpRobot::vpControlFrameType /*frame*/, - vpColVector & /*q*/){}; + void getPosition(const vpRobot::vpControlFrameType /*frame*/, vpColVector & /*q*/){}; public: void init(); @@ -129,12 +124,10 @@ class VISP_EXPORT vpRobotPioneer : public vpRobot, Set a displacement (frame has to be specified) in position control. \warning Not implemented. */ - void setPosition(const vpRobot::vpControlFrameType /*frame*/, - const vpColVector & /*q*/){}; + void setPosition(const vpRobot::vpControlFrameType /*frame*/, const vpColVector & /*q*/){}; public: - void setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &vel); + void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel); /*! Enable or disable sonar device usage. diff --git a/modules/robot/include/visp3/robot/vpRobotPtu46.h b/modules/robot/include/visp3/robot/vpRobotPtu46.h index 37cdf1c6c2..93c1d27df7 100644 --- a/modules/robot/include/visp3/robot/vpRobotPtu46.h +++ b/modules/robot/include/visp3/robot/vpRobotPtu46.h @@ -108,24 +108,20 @@ class VISP_EXPORT vpRobotPtu46 : public vpPtu46, public vpRobot 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); + void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot); vpColVector getVelocity(const vpRobot::vpControlFrameType frame); void init(void); bool readPositionFile(const std::string &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 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 setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot); void stopMotion(); }; diff --git a/modules/robot/include/visp3/robot/vpRobotSimulator.h b/modules/robot/include/visp3/robot/vpRobotSimulator.h index 65ca71ac6e..dbbe6b5ad0 100644 --- a/modules/robot/include/visp3/robot/vpRobotSimulator.h +++ b/modules/robot/include/visp3/robot/vpRobotSimulator.h @@ -88,10 +88,7 @@ class VISP_EXPORT vpRobotSimulator : public vpRobot displacement from the velocity applied to the robot during this time. */ - virtual inline void setSamplingTime(const double &delta_t) - { - this->delta_t_ = delta_t; - } + virtual inline void setSamplingTime(const double &delta_t) { this->delta_t_ = delta_t; } //@} }; diff --git a/modules/robot/include/visp3/robot/vpRobotTemplate.h b/modules/robot/include/visp3/robot/vpRobotTemplate.h index 9206f9ef0b..bfbb4e0f70 100644 --- a/modules/robot/include/visp3/robot/vpRobotTemplate.h +++ b/modules/robot/include/visp3/robot/vpRobotTemplate.h @@ -76,8 +76,7 @@ class VISP_EXPORT vpRobotTemplate : public vpRobot //! 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); + void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel); //! get a position expressed in the robot reference frame void getPosition(vpPoseVector &q); @@ -86,12 +85,10 @@ class VISP_EXPORT vpRobotTemplate : public vpRobot //! 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); + 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); + void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q); }; #endif diff --git a/modules/robot/include/visp3/robot/vpRobotViper650.h b/modules/robot/include/visp3/robot/vpRobotViper650.h index 68c7141b01..41f873c413 100644 --- a/modules/robot/include/visp3/robot/vpRobotViper650.h +++ b/modules/robot/include/visp3/robot/vpRobotViper650.h @@ -310,8 +310,10 @@ frame. To this end this frame is to specify with respect of the end effector frame in \f$^e {\bf M}_c\f$ transformation. This could be done by initializing the robot thanks to init(vpViper650::vpToolType, const vpHomogeneousMatrix &) or init(vpViper650::vpToolType, const std::string &) or using set_eMc(). The -following example illustrates this usecase: \code #include - #include +following example illustrates this usecase: +\code +#include +#include int main() { @@ -404,34 +406,26 @@ class VISP_EXPORT vpRobotViper650 : public vpViper650, public vpRobot */ vpControlModeType getControlMode() const { return controlMode; } - void getDisplacement(vpRobot::vpControlFrameType frame, - vpColVector &displacement); + void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement); void getForceTorque(vpColVector &H) const; vpColVector getForceTorque() 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); + 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); + 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); + vpColVector getVelocity(const vpRobot::vpControlFrameType frame, double ×tamp); void get_cMe(vpHomogeneousMatrix &cMe) const; void get_cVe(vpVelocityTwistMatrix &cVe) const; @@ -439,9 +433,9 @@ class VISP_EXPORT vpRobotViper650 : public vpViper650, public vpRobot void get_fJe(vpMatrix &fJe); void init(void); - void init(vpViper650::vpToolType tool, - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithoutDistortion); + 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_); @@ -462,19 +456,16 @@ class VISP_EXPORT vpRobotViper650 : public vpViper650, public vpRobot 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 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 std::string &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 setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity); void stopMotion(); diff --git a/modules/robot/include/visp3/robot/vpRobotViper850.h b/modules/robot/include/visp3/robot/vpRobotViper850.h index 25d99e5512..ce89d08313 100644 --- a/modules/robot/include/visp3/robot/vpRobotViper850.h +++ b/modules/robot/include/visp3/robot/vpRobotViper850.h @@ -172,8 +172,7 @@ int main() // Set the extrinsic camera parameters obtained with a perpective // projection model including a distortion parameter - robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA, - vpCameraParameters::perspectiveProjWithDistortion); + robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA, vpCameraParameters::perspectiveProjWithDistortion); #endif } \endcode @@ -410,8 +409,7 @@ class VISP_EXPORT vpRobotViper850 : public vpViper850, public vpRobot void disableJoint6Limits() const; void enableJoint6Limits() const; - void getDisplacement(vpRobot::vpControlFrameType frame, - vpColVector &displacement); + 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. @@ -422,26 +420,19 @@ class VISP_EXPORT vpRobotViper850 : public vpViper850, public vpRobot vpColVector getForceTorque() 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); + 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); + 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); + vpColVector getVelocity(const vpRobot::vpControlFrameType frame, double ×tamp); double getTime() const; @@ -451,9 +442,9 @@ class VISP_EXPORT vpRobotViper850 : public vpViper850, public vpRobot void get_fJe(vpMatrix &fJe); void init(void); - void init(vpViper850::vpToolType tool, - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithoutDistortion); + void + init(vpViper850::vpToolType tool, + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithoutDistortion); void init(vpViper850::vpToolType tool, const std::string &filename); void init(vpViper850::vpToolType tool, const vpHomogeneousMatrix &eMc_); @@ -474,11 +465,9 @@ class VISP_EXPORT vpRobotViper850 : public vpViper850, public vpRobot 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 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 std::string &filename); void setPositioningVelocity(const double velocity); @@ -486,8 +475,7 @@ class VISP_EXPORT vpRobotViper850 : public vpViper850, public vpRobot vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState); // Velocity control - void setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &velocity); + void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity); void stopMotion(); void unbiasForceTorqueSensor(); diff --git a/modules/robot/include/visp3/robot/vpRobotWireFrameSimulator.h b/modules/robot/include/visp3/robot/vpRobotWireFrameSimulator.h index c5b5afaf14..0ca8c39c25 100644 --- a/modules/robot/include/visp3/robot/vpRobotWireFrameSimulator.h +++ b/modules/robot/include/visp3/robot/vpRobotWireFrameSimulator.h @@ -46,8 +46,7 @@ #include -#if defined(VISP_HAVE_MODULE_GUI) && \ - ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) +#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) #include // std::fabs #include // numeric_limits @@ -86,8 +85,7 @@ 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 +class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, public vpRobotSimulator { public: vpImage I; @@ -225,17 +223,12 @@ class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, { // 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::epsilon()) && - (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * - std::numeric_limits::epsilon())) - return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, - I.getHeight() / 2); + if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits::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); + return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2); } } /*! @@ -262,8 +255,7 @@ class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, vpHomogeneousMatrix get_fMo() const { return fMo; } /* Display functions */ - void initScene(const vpSceneObject &obj, - const vpSceneDesiredObject &desiredObject); + 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); @@ -284,8 +276,7 @@ class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, \param _constantSamplingTimeMode : The new value of the constantSamplingTimeMode flag. */ - inline void - setConstantSamplingTimeMode(const bool _constantSamplingTimeMode) + inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode) { constantSamplingTimeMode = _constantSamplingTimeMode; } @@ -323,10 +314,7 @@ class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, \param dispType : Type of display. Can be MODEL_3D or MODEL_DH. */ - inline void setDisplayRobotType(const vpDisplayRobotType dispType) - { - displayType = dispType; - } + inline void setDisplayRobotType(const vpDisplayRobotType dispType) { displayType = dispType; } /*! Set the external camera point of view. @@ -340,10 +328,7 @@ class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, /*! Specify the thickness of the graphics drawings. */ - void setGraphicsThickness(unsigned int thickness) - { - this->thickness_ = thickness; - } + void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; } /*! Set the sampling time. @@ -357,10 +342,8 @@ class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, */ inline void setSamplingTime(const double &delta_t) { - if (delta_t < - static_cast(vpTime::getMinTimeForUsleepCall() * 1e-3)) { - this->delta_t_ = - static_cast(vpTime::getMinTimeForUsleepCall() * 1e-3); + if (delta_t < static_cast(vpTime::getMinTimeForUsleepCall() * 1e-3)) { + this->delta_t_ = static_cast(vpTime::getMinTimeForUsleepCall() * 1e-3); } else { this->delta_t_ = delta_t; } @@ -391,15 +374,13 @@ class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, #if defined(_WIN32) static DWORD WINAPI launcher(LPVOID lpParam) { - (static_cast(lpParam)) - ->updateArticularPosition(); + (static_cast(lpParam))->updateArticularPosition(); return 0; } #elif defined(VISP_HAVE_PTHREAD) static void *launcher(void *arg) { - (reinterpret_cast(arg)) - ->updateArticularPosition(); + (reinterpret_cast(arg))->updateArticularPosition(); // pthread_exit((void*) 0); return NULL; } diff --git a/modules/robot/include/visp3/robot/vpServolens.h b/modules/robot/include/visp3/robot/vpServolens.h index c50d8a2750..0f487fdeb5 100644 --- a/modules/robot/include/visp3/robot/vpServolens.h +++ b/modules/robot/include/visp3/robot/vpServolens.h @@ -42,8 +42,7 @@ #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) /*! diff --git a/modules/robot/include/visp3/robot/vpSimulatorAfma6.h b/modules/robot/include/visp3/robot/vpSimulatorAfma6.h index 54c64552be..704c1435ad 100644 --- a/modules/robot/include/visp3/robot/vpSimulatorAfma6.h +++ b/modules/robot/include/visp3/robot/vpSimulatorAfma6.h @@ -49,8 +49,7 @@ #include -#if defined(VISP_HAVE_MODULE_GUI) && \ - ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) +#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) /*! \class vpSimulatorAfma6 @@ -96,8 +95,7 @@ int main() { vpSimulatorAfma6 robot; - robot.init(vpAfma6::TOOL_CCMOP, -vpCameraParameters::perspectiveProjWithoutDistortion); + robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion); vpColVector q(6); // Set a joint position @@ -134,8 +132,7 @@ int main() { vpSimulatorAfma6 robot; - robot.init(vpAfma6::TOOL_GRIPPER, -vpCameraParameters::perspectiveProjWithoutDistortion); + robot.init(vpAfma6::TOOL_GRIPPER, vpCameraParameters::perspectiveProjWithoutDistortion); vpColVector qvel(6); // Set a joint velocity @@ -176,8 +173,7 @@ vpCameraParameters::perspectiveProjWithoutDistortion); */ -class VISP_EXPORT vpSimulatorAfma6 : public vpRobotWireFrameSimulator, - public vpAfma6 +class VISP_EXPORT vpSimulatorAfma6 : public vpRobotWireFrameSimulator, public vpAfma6 { public: static const double defaultPositioningVelocity; @@ -199,37 +195,28 @@ class VISP_EXPORT vpSimulatorAfma6 : public vpRobotWireFrameSimulator, explicit 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 &I); + void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height); + void getCameraParameters(vpCameraParameters &cam, const vpImage &I); void getCameraParameters(vpCameraParameters &cam, const vpImage &I); - void getDisplacement(const vpRobot::vpControlFrameType frame, - vpColVector &displacement); + 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); + 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); + void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp); vpColVector getVelocity(const vpRobot::vpControlFrameType frame); - vpColVector getVelocity(const vpRobot::vpControlFrameType frame, - double ×tamp); + 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); + void + init(vpAfma6::vpAfma6ToolType tool, + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithoutDistortion); bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo); void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo); @@ -238,24 +225,17 @@ class VISP_EXPORT vpSimulatorAfma6 : public vpRobotWireFrameSimulator, static bool readPosFile(const std::string &filename, vpColVector &q); static bool savePosFile(const std::string &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 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 *Iint = NULL, - const double &errMax = 0.001); - vpRobot::vpRobotStateType - setRobotState(const vpRobot::vpRobotStateType newState); - - void setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &velocity); + bool setPosition(const vpHomogeneousMatrix &cdMo, vpImage *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(); diff --git a/modules/robot/include/visp3/robot/vpSimulatorCamera.h b/modules/robot/include/visp3/robot/vpSimulatorCamera.h index 3e1c3c4040..5f6f812f59 100644 --- a/modules/robot/include/visp3/robot/vpSimulatorCamera.h +++ b/modules/robot/include/visp3/robot/vpSimulatorCamera.h @@ -81,18 +81,15 @@ int main() 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; + 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; + 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 + 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; @@ -126,8 +123,7 @@ class VISP_EXPORT vpSimulatorCamera : public vpRobotSimulator 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); + void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel); //@} private: @@ -135,10 +131,8 @@ class VISP_EXPORT vpSimulatorCamera : public vpRobotSimulator // 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 */){}; + 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 index c0cbc4cdad..15cedceda5 100644 --- a/modules/robot/include/visp3/robot/vpSimulatorPioneer.h +++ b/modules/robot/include/visp3/robot/vpSimulatorPioneer.h @@ -80,11 +80,10 @@ int main() 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; + 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.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 @@ -101,8 +100,7 @@ second robot.setMaxTranslationVelocity(1.); // vx max set to 1 m/s tutorial-simu-robot-pioneer. */ -class VISP_EXPORT vpSimulatorPioneer : public vpPioneer, - public vpRobotSimulator +class VISP_EXPORT vpSimulatorPioneer : public vpPioneer, public vpRobotSimulator { protected: @@ -128,8 +126,7 @@ class VISP_EXPORT vpSimulatorPioneer : public vpPioneer, void getPosition(vpHomogeneousMatrix &wMc) const; void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q); - void setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &vel); + void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel); //@} private: @@ -137,10 +134,8 @@ class VISP_EXPORT vpSimulatorPioneer : public vpPioneer, // 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 */){}; + 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 index 1f5b406a36..e7e53a4cf4 100644 --- a/modules/robot/include/visp3/robot/vpSimulatorPioneerPan.h +++ b/modules/robot/include/visp3/robot/vpSimulatorPioneerPan.h @@ -81,11 +81,10 @@ int main() 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; + 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.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 @@ -102,15 +101,13 @@ second robot.setMaxTranslationVelocity(1.); // vx max set to 1 m/s tutorial-simu-robot-pioneer. */ -class VISP_EXPORT vpSimulatorPioneerPan : public vpPioneerPan, - public vpRobotSimulator +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 + 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 @@ -131,8 +128,7 @@ class VISP_EXPORT vpSimulatorPioneerPan : public vpPioneerPan, void getPosition(vpHomogeneousMatrix &wMc) const; void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q); - void setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &vel); + void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel); //@} private: @@ -140,10 +136,8 @@ class VISP_EXPORT vpSimulatorPioneerPan : public vpPioneerPan, // 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 */){}; + 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 index a163352168..ff5ba5b48e 100644 --- a/modules/robot/include/visp3/robot/vpSimulatorViper850.h +++ b/modules/robot/include/visp3/robot/vpSimulatorViper850.h @@ -45,8 +45,7 @@ */ #include -#if defined(VISP_HAVE_MODULE_GUI) && \ - ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) +#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) #include @@ -200,8 +199,7 @@ int main() */ -class VISP_EXPORT vpSimulatorViper850 : public vpRobotWireFrameSimulator, - public vpViper850 +class VISP_EXPORT vpSimulatorViper850 : public vpRobotWireFrameSimulator, public vpViper850 { public: static const double defaultPositioningVelocity; @@ -223,40 +221,31 @@ class VISP_EXPORT vpSimulatorViper850 : public vpRobotWireFrameSimulator, explicit 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 &I); + void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height); + void getCameraParameters(vpCameraParameters &cam, const vpImage &I); void getCameraParameters(vpCameraParameters &cam, const vpImage &I); - void getDisplacement(const vpRobot::vpControlFrameType frame, - vpColVector &displacement); + 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); + 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); + void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp); vpColVector getVelocity(const vpRobot::vpControlFrameType frame); - vpColVector getVelocity(const vpRobot::vpControlFrameType frame, - double ×tamp); + 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); + void + init(vpViper850::vpToolType tool, + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithoutDistortion); bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo); void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo); @@ -266,20 +255,15 @@ class VISP_EXPORT vpSimulatorViper850 : public vpRobotWireFrameSimulator, static bool savePosFile(const std::string &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 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); + vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState); - void setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &velocity); + void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity); void stopMotion(); diff --git a/modules/robot/include/visp3/robot/vpViper.h b/modules/robot/include/visp3/robot/vpViper.h index 43d8c632ac..d315f585c0 100644 --- a/modules/robot/include/visp3/robot/vpViper.h +++ b/modules/robot/include/visp3/robot/vpViper.h @@ -119,12 +119,9 @@ class VISP_EXPORT vpViper /** @name Inherited functionalities from vpViper */ //@{ vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const; - unsigned int getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, - vpColVector &q, + 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; + 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; @@ -140,20 +137,17 @@ class VISP_EXPORT vpViper 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_); + virtual void set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_); vpColVector getJointMin() const; vpColVector getJointMax() const; double getCoupl56() const; //@} - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpViper &viper); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpViper &viper); private: - bool convertJointPositionInLimits(unsigned int joint, const double &q, - double &q_mod, + bool convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod, const bool &verbose = false) const; public: diff --git a/modules/robot/include/visp3/robot/vpViper650.h b/modules/robot/include/visp3/robot/vpViper650.h index 0295fcea63..619b4fd39e 100644 --- a/modules/robot/include/visp3/robot/vpViper650.h +++ b/modules/robot/include/visp3/robot/vpViper650.h @@ -109,8 +109,7 @@ class VISP_EXPORT vpViper650 : public vpViper static const std::string CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME; static const std::string CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME; static const std::string CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME; - static const std::string - CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME; + static const std::string CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME; static const std::string CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME; static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME; static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME; @@ -143,26 +142,19 @@ class VISP_EXPORT vpViper650 : public vpViper //@{ void init(void); void init(const std::string &camera_extrinsic_parameters); - void init(vpViper650::vpToolType tool, - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithoutDistortion); + 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, + 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 &I) const; - void getCameraParameters(vpCameraParameters &cam, - const vpImage &I) const; + void getCameraParameters(vpCameraParameters &cam, const vpImage &I) const; + void getCameraParameters(vpCameraParameters &cam, const vpImage &I) const; //! Get the current tool type vpToolType getToolType() const { return tool_current; }; diff --git a/modules/robot/include/visp3/robot/vpViper850.h b/modules/robot/include/visp3/robot/vpViper850.h index 44889c1a42..82ff869b7d 100644 --- a/modules/robot/include/visp3/robot/vpViper850.h +++ b/modules/robot/include/visp3/robot/vpViper850.h @@ -110,8 +110,7 @@ class VISP_EXPORT vpViper850 : public vpViper static const std::string CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME; static const std::string CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME; static const std::string CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME; - static const std::string - CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME; + static const std::string CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME; static const std::string CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME; static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME; static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME; @@ -144,26 +143,19 @@ class VISP_EXPORT vpViper850 : public vpViper //@{ void init(void); void init(const std::string &camera_extrinsic_parameters); - void init(vpViper850::vpToolType tool, - vpCameraParameters::vpCameraParametersProjType projModel = - vpCameraParameters::perspectiveProjWithoutDistortion); + void + init(vpViper850::vpToolType tool, + vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithoutDistortion); void init(vpViper850::vpToolType tool, const std::string &filename); void init(vpViper850::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, + 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 &I) const; - void getCameraParameters(vpCameraParameters &cam, - const vpImage &I) const; + void getCameraParameters(vpCameraParameters &cam, const vpImage &I) const; + void getCameraParameters(vpCameraParameters &cam, const vpImage &I) const; //! Get the current tool type vpToolType getToolType() const { return tool_current; }; diff --git a/modules/robot/include/visp3/robot/vpVirtuose.h b/modules/robot/include/visp3/robot/vpVirtuose.h index 09e98f79be..78168fc8b9 100644 --- a/modules/robot/include/visp3/robot/vpVirtuose.h +++ b/modules/robot/include/visp3/robot/vpVirtuose.h @@ -127,7 +127,9 @@ metric conventions: - Inertia components in kg.m2 The following sample code shows how to connect to the haptic device to get -its current joint position: \code #include +its current joint position: +\code +#include int main() { diff --git a/modules/robot/include/visp3/robot/vpWireFrameSimulator.h b/modules/robot/include/visp3/robot/vpWireFrameSimulator.h index 21d2cc0c34..5876bd078c 100644 --- a/modules/robot/include/visp3/robot/vpWireFrameSimulator.h +++ b/modules/robot/include/visp3/robot/vpWireFrameSimulator.h @@ -127,8 +127,7 @@ vpImage Iext(480,640,255); //Set the type of scene to use - sim.initScene(vpWireFrameSimulator::PLATE, - vpWireFrameSimulator::D_STANDARD); + 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)); @@ -160,13 +159,13 @@ class VISP_EXPORT vpWireFrameSimulator 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. + 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. @@ -179,22 +178,22 @@ class VISP_EXPORT vpWireFrameSimulator //!< (-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. + //!< (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. + 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 @@ -213,8 +212,8 @@ class VISP_EXPORT vpWireFrameSimulator 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. + 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; @@ -299,14 +298,10 @@ class VISP_EXPORT vpWireFrameSimulator fMoList.clear(); } - void displayTrajectory(const vpImage &I, - const std::list &list_cMo, - const std::list &list_fMo, - const vpHomogeneousMatrix &camMf); - void displayTrajectory(const vpImage &I, - const std::list &list_cMo, - const std::list &list_fMo, - const vpHomogeneousMatrix &camMf); + void displayTrajectory(const vpImage &I, const std::list &list_cMo, + const std::list &list_fMo, const vpHomogeneousMatrix &camMf); + void displayTrajectory(const vpImage &I, const std::list &list_cMo, + const std::list &list_fMo, const vpHomogeneousMatrix &camMf); /*! Get the parameters of the virtual external camera. @@ -315,22 +310,16 @@ class VISP_EXPORT vpWireFrameSimulator \return It returns the camera parameters. */ - vpCameraParameters - getExternalCameraParameters(const vpImage &I) const + vpCameraParameters getExternalCameraParameters(const vpImage &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::epsilon()) && - (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * - std::numeric_limits::epsilon())) - return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, - I.getHeight() / 2); + if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits::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); + return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2); } } /*! @@ -340,22 +329,16 @@ class VISP_EXPORT vpWireFrameSimulator \return It returns the camera parameters. */ - vpCameraParameters - getExternalCameraParameters(const vpImage &I) const + vpCameraParameters getExternalCameraParameters(const vpImage &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::epsilon()) && - (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * - std::numeric_limits::epsilon())) - return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, - I.getHeight() / 2); + if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits::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); + return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2); } } /*! @@ -365,14 +348,10 @@ class VISP_EXPORT vpWireFrameSimulator \return the main external camera position relative to the the world reference frame. */ - inline vpHomogeneousMatrix getExternalCameraPosition() const - { - return rotz * camMf; - } + inline vpHomogeneousMatrix getExternalCameraPosition() const { return rotz * camMf; } void getExternalImage(vpImage &I); - void getExternalImage(vpImage &I, - const vpHomogeneousMatrix &camMf); + void getExternalImage(vpImage &I, const vpHomogeneousMatrix &camMf); void getExternalImage(vpImage &I); void getExternalImage(vpImage &I, const vpHomogeneousMatrix &camMf); @@ -383,22 +362,16 @@ class VISP_EXPORT vpWireFrameSimulator \return It returns the camera parameters. */ - vpCameraParameters - getInternalCameraParameters(const vpImage &I) const + vpCameraParameters getInternalCameraParameters(const vpImage &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::epsilon()) && - (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * - std::numeric_limits::epsilon())) - return vpCameraParameters(px_int, py_int, I.getWidth() / 2, - I.getHeight() / 2); + if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits::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); + return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2); } } /*! @@ -408,22 +381,16 @@ class VISP_EXPORT vpWireFrameSimulator \return It returns the camera parameters. */ - vpCameraParameters - getInternalCameraParameters(const vpImage &I) const + vpCameraParameters getInternalCameraParameters(const vpImage &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::epsilon()) && - (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * - std::numeric_limits::epsilon())) - return vpCameraParameters(px_int, py_int, I.getWidth() / 2, - I.getHeight() / 2); + if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits::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); + return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2); } } @@ -446,8 +413,7 @@ class VISP_EXPORT vpWireFrameSimulator void get_cMo_History(std::list &cMo_history) { cMo_history.clear(); - for (std::list::const_iterator it = poseList.begin(); - it != poseList.end(); ++it) { + for (std::list::const_iterator it = poseList.begin(); it != poseList.end(); ++it) { cMo_history.push_back(rotz * (*it)); } } @@ -465,24 +431,17 @@ class VISP_EXPORT vpWireFrameSimulator \param fMo_history : The list of the homogeneous matrices fMo. */ - void get_fMo_History(std::list &fMo_history) - { - fMo_history = fMoList; - } + void get_fMo_History(std::list &fMo_history) { fMo_history = fMoList; } - void initScene(const vpSceneObject &obj, - const vpSceneDesiredObject &desiredObject); + 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 &imObj); - void initScene(const char *obj, const char *desiredObject, - const std::list &imObj); - void initScene(const vpSceneObject &obj, + void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject, const std::list &imObj); + void initScene(const char *obj, const char *desiredObject, const std::list &imObj); + void initScene(const vpSceneObject &obj, const std::list &imObj); void initScene(const char *obj, const std::list &imObj); /*! @@ -520,10 +479,7 @@ class VISP_EXPORT vpWireFrameSimulator \param factor : The ration for the camera size. */ - inline void setCameraSizeFactor(const float factor) - { - cameraFactor = factor; - } + inline void setCameraSizeFactor(const float factor) { cameraFactor = factor; } /*! Set the color used to display the camera trajectory in the external @@ -540,8 +496,7 @@ class VISP_EXPORT vpWireFrameSimulator \param camTraj_type : The chosen way to display the camera trajectory. */ - inline void setCameraTrajectoryDisplayType( - const vpCameraTrajectoryDisplayType &camTraj_type) + inline void setCameraTrajectoryDisplayType(const vpCameraTrajectoryDisplayType &camTraj_type) { this->camTrajType = camTraj_type; } @@ -557,10 +512,7 @@ class VISP_EXPORT vpWireFrameSimulator \param cdMo_ : The desired pose of the camera. */ - void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_) - { - this->cdMo = rotz * cdMo_; - } + void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_) { this->cdMo = rotz * cdMo_; } /*! Set the color used to display the object at the desired position. @@ -575,10 +527,7 @@ class VISP_EXPORT vpWireFrameSimulator \param do_display : Set to true to display the camera trajectory. */ - void setDisplayCameraTrajectory(const bool &do_display) - { - this->displayCameraTrajectory = do_display; - } + void setDisplayCameraTrajectory(const bool &do_display) { this->displayCameraTrajectory = do_display; } /*! Set the internal camera parameters. @@ -609,10 +558,7 @@ class VISP_EXPORT vpWireFrameSimulator /*! Specify the thickness of the graphics drawings. */ - void setGraphicsThickness(unsigned int thickness) - { - this->thickness_ = thickness; - } + void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; } /*! Set the internal camera parameters. @@ -632,46 +578,31 @@ class VISP_EXPORT vpWireFrameSimulator \param nbPt : The desired number of position which are saved. */ - inline void setNbPtTrajectory(const unsigned int nbPt) - { - nbrPtLimit = nbPt; - } + 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;*/ - } + void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; /*this->cMo = fMc.inverse()*fMo;*/ } //@} protected: /** @name Protected Member Functions Inherited from vpWireFrameSimulator */ //@{ - void display_scene(Matrix mat, Bound_scene &sc, const vpImage &I, - const vpColor &color); - void display_scene(Matrix mat, Bound_scene &sc, - const vpImage &I, const vpColor &color); + void display_scene(Matrix mat, Bound_scene &sc, const vpImage &I, const vpColor &color); + void display_scene(Matrix mat, Bound_scene &sc, const vpImage &I, const vpColor &color); vpHomogeneousMatrix navigation(const vpImage &I, bool &changed); - vpHomogeneousMatrix navigation(const vpImage &I, - bool &changed); - vpImagePoint projectCameraTrajectory(const vpImage &I, - const vpHomogeneousMatrix &cMo, + vpHomogeneousMatrix navigation(const vpImage &I, bool &changed); + vpImagePoint projectCameraTrajectory(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo); - vpImagePoint projectCameraTrajectory(const vpImage &I, - const vpHomogeneousMatrix &cMo, + vpImagePoint projectCameraTrajectory(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo); - vpImagePoint projectCameraTrajectory(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpHomogeneousMatrix &fMo, - const vpHomogeneousMatrix &cMf); - vpImagePoint projectCameraTrajectory(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpHomogeneousMatrix &fMo, - const vpHomogeneousMatrix &cMf); + vpImagePoint projectCameraTrajectory(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf); + vpImagePoint projectCameraTrajectory(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf); //@} }; diff --git a/modules/robot/src/haptic-device/virtuose/vpVirtuose.cpp b/modules/robot/src/haptic-device/virtuose/vpVirtuose.cpp index c6ed99fb86..38677b5800 100644 --- a/modules/robot/src/haptic-device/virtuose/vpVirtuose.cpp +++ b/modules/robot/src/haptic-device/virtuose/vpVirtuose.cpp @@ -52,14 +52,12 @@ * Authorize indexing on all movements by default. */ vpVirtuose::vpVirtuose() - : m_virtContext(NULL), m_ip("localhost#5000"), m_verbose(false), - m_apiMajorVersion(0), m_apiMinorVersion(0), m_ctrlMajorVersion(0), - m_ctrlMinorVersion(0), m_typeCommand(COMMAND_TYPE_IMPEDANCE), - m_indexType(INDEXING_ALL), m_is_init(false), m_period(0.001f) + : m_virtContext(NULL), m_ip("localhost#5000"), m_verbose(false), m_apiMajorVersion(0), m_apiMinorVersion(0), + m_ctrlMajorVersion(0), m_ctrlMinorVersion(0), m_typeCommand(COMMAND_TYPE_IMPEDANCE), m_indexType(INDEXING_ALL), + m_is_init(false), m_period(0.001f) { virtAPIVersion(&m_apiMajorVersion, &m_apiMinorVersion); - std::cout << "API version: " << m_apiMajorVersion << "." - << m_apiMinorVersion << std::endl; + std::cout << "API version: " << m_apiMajorVersion << "." << m_apiMinorVersion << std::endl; } /*! @@ -97,8 +95,7 @@ void vpVirtuose::addForce(vpColVector &force) if (virtAddForce(m_virtContext, virtforce)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtAddForce: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtAddForce: error code %d", err)); } } @@ -112,9 +109,7 @@ void vpVirtuose::enableForceFeedback(int enable) if (virtEnableForceFeedback(m_virtContext, enable)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException( - vpException::fatalError, - "Error calling virtEnableForceFeedback(): error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtEnableForceFeedback(): error code %d", err)); } } @@ -124,8 +119,7 @@ void vpVirtuose::enableForceFeedback(int enable) vpColVector vpVirtuose::getArticularPosition() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } vpColVector articularPosition(6, 0); @@ -133,9 +127,7 @@ vpColVector vpVirtuose::getArticularPosition() const float articular_position_[6]; if (virtGetArticularPosition(m_virtContext, articular_position_)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException( - vpException::fatalError, - "Error calling virtGetArticularPosition(): error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtGetArticularPosition(): error code %d", err)); } for (unsigned int i = 0; i < 6; i++) @@ -150,17 +142,14 @@ vpColVector vpVirtuose::getArticularPosition() const vpColVector vpVirtuose::getArticularVelocity() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } vpColVector articularVelocity(6, 0); float articular_velocity_[6]; if (virtGetArticularSpeed(m_virtContext, articular_velocity_)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtGetArticularSpeed: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtGetArticularSpeed: error code %d", err)); } for (unsigned int i = 0; i < 6; i++) @@ -179,8 +168,7 @@ vpColVector vpVirtuose::getArticularVelocity() const vpPoseVector vpVirtuose::getAvatarPosition() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } float position_[7]; @@ -190,9 +178,7 @@ vpPoseVector vpVirtuose::getAvatarPosition() const if (virtGetAvatarPosition(m_virtContext, position_)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtGetAvatarPosition: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtGetAvatarPosition: error code %d", err)); } else { for (int i = 0; i < 3; i++) translation[i] = position_[i]; @@ -216,8 +202,7 @@ vpPoseVector vpVirtuose::getAvatarPosition() const vpPoseVector vpVirtuose::getBaseFrame() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } vpPoseVector position; @@ -227,8 +212,7 @@ vpPoseVector vpVirtuose::getBaseFrame() const if (virtGetBaseFrame(m_virtContext, position_)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtGetBaseFrame: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtGetBaseFrame: error code %d", err)); } else { for (int i = 0; i < 3; i++) translation[i] = position_[i]; @@ -249,17 +233,14 @@ vpPoseVector vpVirtuose::getBaseFrame() const VirtCommandType vpVirtuose::getCommandType() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } VirtCommandType type; if (virtGetCommandType(m_virtContext, &type)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtGetCommandType: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtGetCommandType: error code %d", err)); } return type; } @@ -271,15 +252,13 @@ VirtCommandType vpVirtuose::getCommandType() const bool vpVirtuose::getDeadMan() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } int deadman; if (virtGetDeadMan(m_virtContext, &deadman)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtGetDeadMan: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtGetDeadMan: error code %d", err)); } return (deadman ? true : false); } @@ -292,16 +271,13 @@ bool vpVirtuose::getDeadMan() const bool vpVirtuose::getEmergencyStop() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } int emergencyStop; if (virtGetEmergencyStop(m_virtContext, &emergencyStop)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtGetEmergencyStop: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtGetEmergencyStop: error code %d", err)); } return (emergencyStop ? true : false); } @@ -313,16 +289,14 @@ bool vpVirtuose::getEmergencyStop() const vpColVector vpVirtuose::getForce() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } vpColVector force(6, 0); float force_[6]; if (virtGetForce(m_virtContext, force_)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtGetForce: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtGetForce: error code %d", err)); } for (unsigned int i = 0; i < 6; i++) @@ -349,9 +323,8 @@ int main() VirtContext handler = virtuose.getHandler(); float q[6]; - if (virtGetArticularPosition(handler, q)) { // Use the handler to access to -Haption API directly std::cout << "Cannot get articular position" << -std::endl; + if (virtGetArticularPosition(handler, q)) { // Use the handler to access to Haption API directly + std::cout << "Cannot get articular position" << std::endl; } std::cout << "Joint position: "; for (unsigned int i=0; i<6; i++) @@ -374,8 +347,7 @@ VirtContext vpVirtuose::getHandler() { return m_virtContext; } vpPoseVector vpVirtuose::getObservationFrame() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } vpPoseVector position; @@ -385,9 +357,7 @@ vpPoseVector vpVirtuose::getObservationFrame() const if (virtGetObservationFrame(m_virtContext, position_)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtGetObservationFrame: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtGetObservationFrame: error code %d", err)); } else { for (int i = 0; i < 3; i++) translation[i] = position_[i]; @@ -409,8 +379,7 @@ vpPoseVector vpVirtuose::getObservationFrame() const vpPoseVector vpVirtuose::getPhysicalPosition() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } vpPoseVector position; @@ -420,9 +389,7 @@ vpPoseVector vpVirtuose::getPhysicalPosition() const if (virtGetPhysicalPosition(m_virtContext, position_)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtGetPhysicalPosition: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtGetPhysicalPosition: error code %d", err)); } else { for (int i = 0; i < 3; i++) translation[i] = position_[i]; @@ -445,16 +412,14 @@ vpPoseVector vpVirtuose::getPhysicalPosition() const vpColVector vpVirtuose::getPhysicalVelocity() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } vpColVector vel(6, 0); float speed[6]; if (virtGetPhysicalSpeed(m_virtContext, speed)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtGetPhysicalSpeed: error code %s", + throw(vpException(vpException::fatalError, "Error calling virtGetPhysicalSpeed: error code %s", virtGetErrorMessage(err))); } for (unsigned int i = 0; i < 6; i++) @@ -470,8 +435,7 @@ vpColVector vpVirtuose::getPhysicalVelocity() const vpPoseVector vpVirtuose::getPosition() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } vpPoseVector position; @@ -481,8 +445,7 @@ vpPoseVector vpVirtuose::getPosition() const if (virtGetPosition(m_virtContext, position_)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtGetPosition: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtGetPosition: error code %d", err)); } else { for (int i = 0; i < 3; i++) translation[i] = position_[i]; @@ -502,8 +465,7 @@ vpPoseVector vpVirtuose::getPosition() const bool vpVirtuose::getPower() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } int power; @@ -520,17 +482,14 @@ bool vpVirtuose::getPower() const vpColVector vpVirtuose::getVelocity() const { if (!m_is_init) { - throw(vpException(vpException::fatalError, - "Device not initialized. Call init().")); + throw(vpException(vpException::fatalError, "Device not initialized. Call init().")); } vpColVector vel(6, 0); float speed[6]; if (virtGetSpeed(m_virtContext, speed)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Cannot get haptic device velocity: %s", - virtGetErrorMessage(err))); + throw(vpException(vpException::fatalError, "Cannot get haptic device velocity: %s", virtGetErrorMessage(err))); } for (unsigned int i = 0; i < 6; i++) vel[i] = speed[i]; @@ -549,35 +508,28 @@ void vpVirtuose::init() if (m_virtContext == NULL) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Cannot open haptic device: %s", - virtGetErrorMessage(err))); + throw(vpException(vpException::fatalError, "Cannot open haptic device: %s", virtGetErrorMessage(err))); } - if (virtGetControlerVersion(m_virtContext, &m_ctrlMajorVersion, - &m_ctrlMinorVersion)) { + if (virtGetControlerVersion(m_virtContext, &m_ctrlMajorVersion, &m_ctrlMinorVersion)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Cannot get haptic device controller version: %s", + throw(vpException(vpException::fatalError, "Cannot get haptic device controller version: %s", virtGetErrorMessage(err))); } if (m_verbose) { - std::cout << "Controller version: " << m_ctrlMajorVersion << "." - << m_ctrlMinorVersion << std::endl; + std::cout << "Controller version: " << m_ctrlMajorVersion << "." << m_ctrlMinorVersion << std::endl; } if (virtSetCommandType(m_virtContext, m_typeCommand)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Cannot set haptic device command type: %s", - virtGetErrorMessage(err))); + throw( + vpException(vpException::fatalError, "Cannot set haptic device command type: %s", virtGetErrorMessage(err))); } if (virtSetTimeStep(m_virtContext, m_period)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetTimeStep: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtSetTimeStep: error code %d", err)); } m_is_init = true; @@ -607,9 +559,7 @@ void vpVirtuose::setArticularForce(const vpColVector &articularForce) if (virtSetArticularForce(m_virtContext, articular_force)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetArticularForce: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtSetArticularForce: error code %d", err)); } } @@ -636,9 +586,7 @@ void vpVirtuose::setArticularPosition(const vpColVector &articularPosition) if (virtSetArticularPosition(m_virtContext, articular_position)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetArticularPosition: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtSetArticularPosition: error code %d", err)); } } @@ -665,9 +613,7 @@ void vpVirtuose::setArticularVelocity(const vpColVector &articularVelocity) if (virtSetArticularSpeed(m_virtContext, articular_velocity)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetArticularVelocity: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtSetArticularVelocity: error code %d", err)); } } @@ -696,8 +642,7 @@ void vpVirtuose::setBaseFrame(const vpPoseVector &position) if (virtSetBaseFrame(m_virtContext, position_)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetBaseFrame: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtSetBaseFrame: error code %d", err)); } } @@ -720,9 +665,7 @@ void vpVirtuose::setCommandType(const VirtCommandType &type) if (virtSetCommandType(m_virtContext, m_typeCommand)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetCommandType: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtSetCommandType: error code %d", err)); } } } @@ -750,8 +693,7 @@ void vpVirtuose::setForce(const vpColVector &force) if (virtSetForce(m_virtContext, virtforce)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetForce: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtSetForce: error code %d", err)); } } @@ -766,9 +708,7 @@ void vpVirtuose::setForceFactor(const float &forceFactor) if (virtSetForceFactor(m_virtContext, forceFactor)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetForceFactor: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtSetForceFactor: error code %d", err)); } } @@ -795,8 +735,7 @@ void vpVirtuose::setIndexingMode(const VirtIndexingType &type) if (virtSetIndexingMode(m_virtContext, m_indexType)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling setIndexingMode: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling setIndexingMode: error code %d", err)); } } } @@ -826,9 +765,7 @@ void vpVirtuose::setObservationFrame(const vpPoseVector &position) if (virtSetObservationFrame(m_virtContext, position_)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetObservationFrame: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtSetObservationFrame: error code %d", err)); } } @@ -877,9 +814,7 @@ void vpVirtuose::setPeriodicFunction(VirtPeriodicFunction CallBackVirt) if (virtSetPeriodicFunction(m_virtContext, CallBackVirt, &m_period, this)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetPeriodicFunction: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtSetPeriodicFunction: error code %d", err)); } } @@ -906,8 +841,7 @@ void vpVirtuose::setPosition(vpPoseVector &position) if (virtSetPosition(m_virtContext, position_)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetPosition: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtSetPosition: error code %d", err)); } } @@ -920,8 +854,7 @@ void vpVirtuose::setPowerOff() if (virtSetPowerOn(m_virtContext, 0)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetPowerOff: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtSetPowerOff: error code %d", err)); } } @@ -934,8 +867,7 @@ void vpVirtuose::setPowerOn() if (virtSetPowerOn(m_virtContext, 1)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetPowerOn: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtSetPowerOn: error code %d", err)); } } @@ -944,16 +876,13 @@ void vpVirtuose::setPowerOn() * \param forceLimit : Value expressed in N. * \param torqueLimit : Value expressed in Nm. */ -void vpVirtuose::setSaturation(const float &forceLimit, - const float &torqueLimit) +void vpVirtuose::setSaturation(const float &forceLimit, const float &torqueLimit) { init(); if (virtSaturateTorque(m_virtContext, forceLimit, torqueLimit)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSaturateTorque: error code %d", - err)); + throw(vpException(vpException::fatalError, "Error calling virtSaturateTorque: error code %d", err)); } } @@ -971,8 +900,7 @@ void vpVirtuose::setTimeStep(const float &timeStep) if (virtSetTimeStep(m_virtContext, m_period)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetTimeStep: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtSetTimeStep: error code %d", err)); } } } @@ -987,10 +915,8 @@ void vpVirtuose::setVelocity(vpColVector &velocity) init(); if (velocity.size() != 6) { - throw(vpException( - vpException::dimensionError, - "Cannot set a velocity vector (dim %d) that is not 6-dimension", - velocity.size())); + throw(vpException(vpException::dimensionError, "Cannot set a velocity vector (dim %d) that is not 6-dimension", + velocity.size())); } float speed[6]; @@ -999,8 +925,7 @@ void vpVirtuose::setVelocity(vpColVector &velocity) if (virtSetSpeed(m_virtContext, speed)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling virtSetSpeed: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling virtSetSpeed: error code %d", err)); } } @@ -1015,8 +940,7 @@ void vpVirtuose::setVelocityFactor(const float &velocityFactor) if (virtSetSpeedFactor(m_virtContext, velocityFactor)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling setVelocityFactor: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling setVelocityFactor: error code %d", err)); } } @@ -1031,8 +955,7 @@ void vpVirtuose::startPeriodicFunction() if (virtStartLoop(m_virtContext)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling startLoop: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling startLoop: error code %d", err)); } else std::cout << "Haptic loop open." << std::endl; } @@ -1048,8 +971,7 @@ void vpVirtuose::stopPeriodicFunction() if (virtStopLoop(m_virtContext)) { int err = virtGetErrorCode(m_virtContext); - throw(vpException(vpException::fatalError, - "Error calling stopLoop: error code %d", err)); + throw(vpException(vpException::fatalError, "Error calling stopLoop: error code %d", err)); } else std::cout << "Haptic loop closed." << std::endl; } diff --git a/modules/robot/src/image-simulator/vpImageSimulator.cpp b/modules/robot/src/image-simulator/vpImageSimulator.cpp index fe3bc28994..ef3f16e91d 100644 --- a/modules/robot/src/image-simulator/vpImageSimulator.cpp +++ b/modules/robot/src/image-simulator/vpImageSimulator.cpp @@ -60,13 +60,10 @@ 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) + : 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); @@ -100,13 +97,11 @@ vpImageSimulator::vpImageSimulator(const vpColorPlan &col) 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) + : 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++) { @@ -191,17 +186,14 @@ vpImageSimulator &vpImageSimulator::operator=(const vpImageSimulator &sim) 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 &I, - const vpCameraParameters &cam) +void vpImageSimulator::getImage(vpImage &I, const vpCameraParameters &cam) { 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); + 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; @@ -227,8 +219,7 @@ void vpImageSimulator::getImage(vpImage &I, int nb_point_dessine = 0; for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) { - for (unsigned int j = (unsigned int)left; j < (unsigned int)right; - j++) { + 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); @@ -242,9 +233,8 @@ void vpImageSimulator::getImage(vpImage &I, } 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); + 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++; } @@ -263,14 +253,10 @@ void vpImageSimulator::getImage(vpImage &I, \param Isrc : The image which is projected into \f$ I \f$. \param cam : The parameters of the virtual camera. */ -void vpImageSimulator::getImage(vpImage &I, - vpImage &Isrc, - const vpCameraParameters &cam) +void vpImageSimulator::getImage(vpImage &I, vpImage &Isrc, const vpCameraParameters &cam) { if (cleanPrevImage) { - unsigned char col = - (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + - 0.0722 * bgColor.B); + 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; @@ -294,8 +280,7 @@ void vpImageSimulator::getImage(vpImage &I, int nb_point_dessine = 0; for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) { - for (unsigned int j = (unsigned int)left; j < (unsigned int)right; - j++) { + 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); @@ -325,20 +310,14 @@ void vpImageSimulator::getImage(vpImage &I, \param zBuffer : A matrix containing the z coordinates of the pixels of the image \f$ I \f$ */ -void vpImageSimulator::getImage(vpImage &I, - const vpCameraParameters &cam, - vpMatrix &zBuffer) +void vpImageSimulator::getImage(vpImage &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 ! ")); + 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 ! ")); if (cleanPrevImage) { - unsigned char col = - (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + - 0.0722 * bgColor.B); + 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; @@ -362,8 +341,7 @@ void vpImageSimulator::getImage(vpImage &I, int nb_point_dessine = 0; for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) { - for (unsigned int j = (unsigned int)left; j < (unsigned int)right; - j++) { + 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); @@ -382,9 +360,7 @@ void vpImageSimulator::getImage(vpImage &I, 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); + (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]; @@ -403,8 +379,7 @@ void vpImageSimulator::getImage(vpImage &I, \param I : The image used to store the result. \param cam : The parameters of the virtual camera. */ -void vpImageSimulator::getImage(vpImage &I, - const vpCameraParameters &cam) +void vpImageSimulator::getImage(vpImage &I, const vpCameraParameters &cam) { if (cleanPrevImage) { for (unsigned int i = 0; i < I.getHeight(); i++) { @@ -431,8 +406,7 @@ void vpImageSimulator::getImage(vpImage &I, int nb_point_dessine = 0; for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) { - for (unsigned int j = (unsigned int)left; j < (unsigned int)right; - j++) { + 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); @@ -468,8 +442,7 @@ void vpImageSimulator::getImage(vpImage &I, \param Isrc : The image which is projected into \f$ I \f$. \param cam : The parameters of the virtual camera. */ -void vpImageSimulator::getImage(vpImage &I, vpImage &Isrc, - const vpCameraParameters &cam) +void vpImageSimulator::getImage(vpImage &I, vpImage &Isrc, const vpCameraParameters &cam) { if (cleanPrevImage) { for (unsigned int i = 0; i < I.getHeight(); i++) { @@ -496,8 +469,7 @@ void vpImageSimulator::getImage(vpImage &I, vpImage &Isrc, int nb_point_dessine = 0; for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) { - for (unsigned int j = (unsigned int)left; j < (unsigned int)right; - j++) { + 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); @@ -527,15 +499,11 @@ void vpImageSimulator::getImage(vpImage &I, vpImage &Isrc, \param zBuffer : A matrix containing the z coordinates of the pixels of the image \f$ I \f$ */ -void vpImageSimulator::getImage(vpImage &I, - const vpCameraParameters &cam, - vpMatrix &zBuffer) +void vpImageSimulator::getImage(vpImage &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 ! ")); + 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 ! ")); if (cleanPrevImage) { for (unsigned int i = 0; i < I.getHeight(); i++) { @@ -561,8 +529,7 @@ void vpImageSimulator::getImage(vpImage &I, int nb_point_dessine = 0; for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) { - for (unsigned int j = (unsigned int)left; j < (unsigned int)right; - j++) { + 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); @@ -699,8 +666,7 @@ void vpImageSimulator::getImage(vpImage &I, \param list : List of vpImageSimulator to project \param cam : The parameters of the virtual camera */ -void vpImageSimulator::getImage(vpImage &I, - std::list &list, +void vpImageSimulator::getImage(vpImage &I, std::list &list, const vpCameraParameters &cam) { @@ -722,8 +688,7 @@ void vpImageSimulator::getImage(vpImage &I, unsigned int unvisible = 0; unsigned int indexSimu = 0; - for (std::list::iterator it = list.begin(); - it != list.end(); ++it, ++indexSimu) { + for (std::list::iterator it = list.begin(); it != list.end(); ++it, ++indexSimu) { vpImageSimulator *sim = &(*it); if (sim->visible) simList[indexSimu] = sim; @@ -739,11 +704,9 @@ void vpImageSimulator::getImage(vpImage &I, for (unsigned int i = 0; i < nbsimList; i++) { if (!simList[i]->needClipping) - simList[i]->getRoi(width, height, cam, simList[i]->pt, - simList[i]->rect); + 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); + simList[i]->getRoi(width, height, cam, simList[i]->ptClipped, simList[i]->rect); if (topFinal > simList[i]->rect.getTop()) topFinal = simList[i]->rect.getTop(); @@ -760,10 +723,8 @@ void vpImageSimulator::getImage(vpImage &I, 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++) { + 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); @@ -787,8 +748,7 @@ void vpImageSimulator::getImage(vpImage &I, 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); + (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B); *(bitmap + i * width + j) = pixelgrey; } } @@ -904,9 +864,7 @@ void vpImageSimulator::getImage(vpImage &I, \param list : List of vpImageSimulator to project \param cam : The parameters of the virtual camera */ -void vpImageSimulator::getImage(vpImage &I, - std::list &list, - const vpCameraParameters &cam) +void vpImageSimulator::getImage(vpImage &I, std::list &list, const vpCameraParameters &cam) { unsigned int width = I.getWidth(); @@ -927,8 +885,7 @@ void vpImageSimulator::getImage(vpImage &I, unsigned int unvisible = 0; unsigned int indexSimu = 0; - for (std::list::iterator it = list.begin(); - it != list.end(); ++it, ++indexSimu) { + for (std::list::iterator it = list.begin(); it != list.end(); ++it, ++indexSimu) { vpImageSimulator *sim = &(*it); if (sim->visible) simList[indexSimu] = sim; @@ -945,11 +902,9 @@ void vpImageSimulator::getImage(vpImage &I, for (unsigned int i = 0; i < nbsimList; i++) { if (!simList[i]->needClipping) - simList[i]->getRoi(width, height, cam, simList[i]->pt, - simList[i]->rect); + 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); + simList[i]->getRoi(width, height, cam, simList[i]->ptClipped, simList[i]->rect); if (topFinal > simList[i]->rect.getTop()) topFinal = simList[i]->rect.getTop(); @@ -966,10 +921,8 @@ void vpImageSimulator::getImage(vpImage &I, 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++) { + 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); @@ -1040,9 +993,7 @@ void vpImageSimulator::setCameraPosition(const vpHomogeneousMatrix &cMt_) 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]; + 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; @@ -1077,8 +1028,7 @@ void vpImageSimulator::setCameraPosition(const vpHomogeneousMatrix &cMt_) std::vector *ptPtr = &pt; if (needClipping) { - vpPolygon3D::getClippedPolygon(pt, ptClipped, cMt, - vpPolygon3D::NEAR_CLIPPING); + vpPolygon3D::getClippedPolygon(pt, ptClipped, cMt, vpPolygon3D::NEAR_CLIPPING); ptPtr = &ptClipped; } @@ -1195,13 +1145,10 @@ void vpImageSimulator::init(const char *file_image, vpColVector *X_) \param X_ : Vector of the 3D coordinates in the object frame (oX, oY, oZ) corresponding to the image corners. */ -void vpImageSimulator::init(const vpImage &I, - const std::vector &X_) +void vpImageSimulator::init(const vpImage &I, const std::vector &X_) { if (X_.size() != 4) { - throw vpException( - vpException::dimensionError, - "the vector must contains 4 points to initialise the simulator"); + 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) { @@ -1230,13 +1177,10 @@ void vpImageSimulator::init(const vpImage &I, \param X_ : Vector of the 3D coordinates in the object frame (oX, oY, oZ) corresponding to the image corners. */ -void vpImageSimulator::init(const vpImage &I, - const std::vector &X_) +void vpImageSimulator::init(const vpImage &I, const std::vector &X_) { if (X_.size() != 4) { - throw vpException( - vpException::dimensionError, - "the vector must contains 4 points to initialise the simulator"); + 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) { @@ -1268,13 +1212,10 @@ void vpImageSimulator::init(const vpImage &I, \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 &X_) +void vpImageSimulator::init(const char *file_image, const std::vector &X_) { if (X_.size() != 4) { - throw vpException( - vpException::dimensionError, - "the vector must contains 4 points to initialise the simulator"); + 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) { @@ -1290,8 +1231,7 @@ void vpImageSimulator::init(const char *file_image, } #endif -bool vpImageSimulator::getPixel(const vpImagePoint &iP, - unsigned char &Ipixelplan) +bool vpImageSimulator::getPixel(const vpImagePoint &iP, unsigned char &Ipixelplan) { // std::cout << "In get Pixel" << std::endl; // test si pixel dans zone projetee @@ -1306,15 +1246,14 @@ bool vpImageSimulator::getPixel(const vpImagePoint &iP, // if(!T1.inTriangle(iP) && !T2.inTriangle(iP)){ //// std::cout << "The pixel is inside the projected area" << - ///std::endl; + /// 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]); + 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; @@ -1348,9 +1287,7 @@ bool vpImageSimulator::getPixel(const vpImagePoint &iP, return false; } -bool vpImageSimulator::getPixel(vpImage &Isrc, - const vpImagePoint &iP, - unsigned char &Ipixelplan) +bool vpImageSimulator::getPixel(vpImage &Isrc, const vpImagePoint &iP, unsigned char &Ipixelplan) { // test si pixel dans zone projetee bool inside = false; @@ -1369,8 +1306,7 @@ bool vpImageSimulator::getPixel(vpImage &Isrc, 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]); + 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; @@ -1422,8 +1358,7 @@ bool vpImageSimulator::getPixel(const vpImagePoint &iP, vpRGBa &Ipixelplan) 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]); + 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; @@ -1457,8 +1392,7 @@ bool vpImageSimulator::getPixel(const vpImagePoint &iP, vpRGBa &Ipixelplan) return false; } -bool vpImageSimulator::getPixel(vpImage &Isrc, const vpImagePoint &iP, - vpRGBa &Ipixelplan) +bool vpImageSimulator::getPixel(vpImage &Isrc, const vpImagePoint &iP, vpRGBa &Ipixelplan) { // test si pixel dans zone projetee // if(!T1.inTriangle(iP) && !T2.inTriangle(iP)) @@ -1476,8 +1410,7 @@ bool vpImageSimulator::getPixel(vpImage &Isrc, const vpImagePoint &iP, 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]); + 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; @@ -1511,8 +1444,7 @@ bool vpImageSimulator::getPixel(vpImage &Isrc, const vpImagePoint &iP, return false; } -bool vpImageSimulator::getPixelDepth(const vpImagePoint &iP, - double &Zpixelplan) +bool vpImageSimulator::getPixelDepth(const vpImagePoint &iP, double &Zpixelplan) { // test si pixel dans zone projetee bool inside = false; @@ -1526,14 +1458,11 @@ bool vpImageSimulator::getPixelDepth(const vpImagePoint &iP, // 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]); + 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) +bool vpImageSimulator::getPixelVisibility(const vpImagePoint &iP, double &Visipixelplan) { // test si pixel dans zone projetee bool inside = false; @@ -1551,9 +1480,7 @@ bool vpImageSimulator::getPixelVisibility(const vpImagePoint &iP, return true; } -void vpImageSimulator::project(const vpColVector &_vin, - const vpHomogeneousMatrix &_cMt, - vpColVector &_vout) +void vpImageSimulator::project(const vpColVector &_vin, const vpHomogeneousMatrix &_cMt, vpColVector &_vout) { vpColVector XH(4); getHomogCoord(_vin, XH); @@ -1567,18 +1494,14 @@ void vpImageSimulator::getHomogCoord(const vpColVector &_v, vpColVector &_vH) _vH[3] = 1.; } -void vpImageSimulator::getCoordFromHomog(const vpColVector &_vH, - vpColVector &_v) +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 &point, - vpRect &rectangle) +void vpImageSimulator::getRoi(const unsigned int &Iwidth, const unsigned int &Iheight, const vpCameraParameters &cam, + const std::vector &point, vpRect &rectangle) { double top = Iheight + 1; double bottom = -1; @@ -1587,8 +1510,7 @@ void vpImageSimulator::getRoi(const unsigned int &Iwidth, 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); + vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), u, v); if (v < top) top = v; if (v > bottom) @@ -1629,8 +1551,7 @@ std::vector vpImageSimulator::get3DcornersTextureRectangle() return X_; } -VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpImageSimulator & /*ip*/) +VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpImageSimulator & /*ip*/) { os << ""; return os; diff --git a/modules/robot/src/real-robot/afma4/vpAfma4.cpp b/modules/robot/src/real-robot/afma4/vpAfma4.cpp index 7b770b762a..f95af8d890 100644 --- a/modules/robot/src/real-robot/afma4/vpAfma4.cpp +++ b/modules/robot/src/real-robot/afma4/vpAfma4.cpp @@ -306,10 +306,7 @@ void vpAfma4::get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const end-effector frame. */ -void vpAfma4::get_cMe(vpHomogeneousMatrix &cMe) const -{ - cMe = this->_eMc.inverse(); -} +void vpAfma4::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); } /*! @@ -506,8 +503,7 @@ void vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const \sa get_eJe() and get_fJe() */ -void vpAfma4::get_fJe_inverse(const vpColVector &q, - vpMatrix &fJe_inverse) const +void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const { fJe_inverse.resize(4, 6); fJe_inverse = 0; @@ -582,14 +578,12 @@ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma4 &afma4) 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 + << "\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 + << "\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 @@ -601,13 +595,11 @@ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma4 &afma4) << "\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; + << "\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 index e5aed26f2d..6ebb575ce5 100644 --- a/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp +++ b/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp @@ -81,8 +81,7 @@ const double vpRobotAfma4::defaultPositioningVelocity = 15.0; */ void emergencyStopAfma4(int signo) { - std::cout << "Stop the Afma4 application by signal (" << signo - << "): " << (char)7; + std::cout << "Stop the Afma4 application by signal (" << signo << "): " << (char)7; switch (signo) { case SIGINT: std::cout << "SIGINT (stop by ^C) " << std::endl; @@ -209,8 +208,7 @@ void vpRobotAfma4::init(void) if (stt != SUCCESS) { vpERROR_TRACE("Cannot open connection with the motionblox."); - throw vpRobotException(vpRobotException::constructionError, - "Cannot open connection with the motionblox"); + throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox"); } // Connect to the servoboard using the servo board GUID @@ -218,16 +216,14 @@ void vpRobotAfma4::init(void) if (stt != SUCCESS) { vpERROR_TRACE("Cannot open connection with the motionblox."); - throw vpRobotException(vpRobotException::constructionError, - "Cannot open connection with the motionblox"); + throw vpRobotException(vpRobotException::constructionError, "Cannot open connection 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)); + Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); // Print the robot status @@ -245,8 +241,7 @@ void vpRobotAfma4::init(void) 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 << "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 } @@ -278,8 +273,7 @@ void vpRobotAfma4::init(void) ShutDownConnection(); std::cout << "Cannot open connection with the motionblox..." << std::endl; - throw vpRobotException(vpRobotException::constructionError, - "Cannot open connection with the motionblox"); + throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox"); } return; } @@ -302,8 +296,7 @@ vpRobotAfma4::~vpRobotAfma4(void) // Look if the power is on or off UInt32 HIPowerStatus; - Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); // if (HIPowerStatus == 1) { @@ -328,8 +321,7 @@ Change the robot state. \param newState : New requested robot state. */ -vpRobot::vpRobotStateType -vpRobotAfma4::setRobotState(vpRobot::vpRobotStateType newState) +vpRobot::vpRobotStateType vpRobotAfma4::setRobotState(vpRobot::vpRobotStateType newState) { InitTry; @@ -342,8 +334,7 @@ vpRobotAfma4::setRobotState(vpRobot::vpRobotStateType newState) } case vpRobot::STATE_POSITION_CONTROL: { if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) { - std::cout - << "Change the control mode from velocity to position control.\n"; + 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 @@ -388,8 +379,7 @@ void vpRobotAfma4::stopMotion(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot stop robot motion"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot stop robot motion."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion."); } } @@ -413,8 +403,7 @@ void vpRobotAfma4::powerOn(void) unsigned int nitermax = 10; for (unsigned int i = 0; i < nitermax; i++) { - Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus)); if (EStopStatus == ESTOP_AUTO) { break; // exit for loop } else if (EStopStatus == ESTOP_MANUAL) { @@ -431,8 +420,7 @@ void vpRobotAfma4::powerOn(void) fflush(stdout); CAL_Wait(1); } else { - std::cout << "Sorry there is an error on the emergency chain." - << std::endl; + 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(); @@ -445,8 +433,7 @@ void vpRobotAfma4::powerOn(void) if (EStopStatus == ESTOP_ACTIVATED) { std::cout << "Sorry, cannot power on the robot." << std::endl; - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot power on the robot."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."); } if (HIPowerStatus == 0) { @@ -459,8 +446,7 @@ void vpRobotAfma4::powerOn(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot power on the robot"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot power off the robot."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."); } } @@ -479,8 +465,7 @@ void vpRobotAfma4::powerOff(void) // Look if the power is on or off UInt32 HIPowerStatus; - Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); if (HIPowerStatus == 1) { @@ -493,8 +478,7 @@ void vpRobotAfma4::powerOff(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot power off the robot"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot power off the robot."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."); } } @@ -515,8 +499,7 @@ bool vpRobotAfma4::getPowerState(void) bool status = false; // Look if the power is on or off UInt32 HIPowerStatus; - Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); if (HIPowerStatus == 1) { @@ -526,8 +509,7 @@ bool vpRobotAfma4::getPowerState(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get the power status"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get the power status."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status."); } return status; } @@ -589,10 +571,7 @@ void vpRobotAfma4::get_cVf(vpVelocityTwistMatrix &cVf) const end-effector frame. */ -void vpRobotAfma4::get_cMe(vpHomogeneousMatrix &cMe) const -{ - vpAfma4::get_cMe(cMe); -} +void vpRobotAfma4::get_cMe(vpHomogeneousMatrix &cMe) const { vpAfma4::get_cMe(cMe); } /*! @@ -690,20 +669,14 @@ void vpRobotAfma4::get_fJe(vpMatrix &fJe) \sa getPositioningVelocity() */ -void vpRobotAfma4::setPositioningVelocity(const double velocity) -{ - positioningVelocity = velocity; -} +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; -} +double vpRobotAfma4::getPositioningVelocity(void) { return positioningVelocity; } /*! @@ -777,8 +750,7 @@ double vpRobotAfma4::getPositioningVelocity(void) */ -void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, - const vpColVector &position) +void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) { if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) { @@ -792,21 +764,18 @@ void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, switch (frame) { case vpRobot::REFERENCE_FRAME: vpERROR_TRACE("Positionning error. Reference frame not implemented"); - throw vpRobotException(vpRobotException::lowLevelError, - "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."); + 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."); + throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: " + "Mixt frame not implemented."); break; case vpRobot::ARTICULAR_FRAME: { @@ -815,8 +784,7 @@ void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, } if (position.getRows() != this->njoint) { vpERROR_TRACE("Positionning error: bad vector dimension."); - throw vpRobotException(vpRobotException::positionOutOfRangeError, - "Positionning error: bad vector dimension."); + throw vpRobotException(vpRobotException::positionOutOfRangeError, "Positionning error: bad vector dimension."); } InitTry; @@ -834,8 +802,7 @@ void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, if (TryStt < 0 || error < 0) { vpERROR_TRACE("Positionning error."); - throw vpRobotException(vpRobotException::positionOutOfRangeError, - "Position out of range."); + throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range."); } return; @@ -891,8 +858,7 @@ void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, \sa setPosition() */ -void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, - const double q1, const double q2, +void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, const double q1, const double q2, const double q4, const double q5) { try { @@ -947,8 +913,7 @@ void vpRobotAfma4::setPosition(const char *filename) if (ret == false) { vpERROR_TRACE("Bad position in \"%s\"", filename); - throw vpRobotException(vpRobotException::lowLevelError, - "Bad position in filename."); + throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename."); } this->setRobotState(vpRobot::STATE_POSITION_CONTROL); this->setPosition(vpRobot::ARTICULAR_FRAME, q); @@ -1025,8 +990,7 @@ double vpRobotAfma4::getTime() const vpColVector & r) */ -void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &position, double ×tamp) +void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double ×tamp) { InitTry; @@ -1073,17 +1037,15 @@ void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, } 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"); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: " + "not implemented"); } } CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get position."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get position."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position."); } return; @@ -1099,8 +1061,7 @@ void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, The difference is here that the timestamp is not used. */ -void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &position) +void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) { double timestamp; getPosition(frame, position, timestamp); @@ -1159,18 +1120,15 @@ void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot); \endcode */ -void vpRobotAfma4::setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &vel) +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) "); + 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 @@ -1180,18 +1138,16 @@ void vpRobotAfma4::setVelocity(const vpRobot::vpControlFrameType 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"); + 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 "); + throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the articular " + "velocity vector "); } break; } @@ -1199,25 +1155,22 @@ void vpRobotAfma4::setVelocity(const vpRobot::vpControlFrameType 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot " + "in the reference frame:" + "functionality not implemented"); } 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot " + "in the mixt frame:" + "functionality not implemented"); } default: { vpERROR_TRACE("Error in spec of vpRobot. " "Case not taken in account."); - throw vpRobotException(vpRobotException::wrongStateError, - "Cannot send a velocity to the robot "); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "); } } @@ -1293,8 +1246,7 @@ void vpRobotAfma4::setVelocity(const vpRobot::vpControlFrameType frame, 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; + std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl; } } else { printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt); @@ -1361,8 +1313,7 @@ void vpRobotAfma4::setVelocity(const vpRobot::vpControlFrameType frame, } \endcode */ -void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &velocity, double ×tamp) +void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double ×tamp) { switch (frame) { @@ -1430,9 +1381,8 @@ void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the mixt frame:" + "functionality not implemented"); break; } @@ -1453,8 +1403,7 @@ void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get velocity."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get velocity."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity."); } } @@ -1466,8 +1415,7 @@ void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &, double &) except that the timestamp is not returned. */ -void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &velocity) +void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity) { double timestamp; getVelocity(frame, velocity, timestamp); @@ -1512,8 +1460,7 @@ void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, } \endcode */ -vpColVector vpRobotAfma4::getVelocity(vpRobot::vpControlFrameType frame, - double ×tamp) +vpColVector vpRobotAfma4::getVelocity(vpRobot::vpControlFrameType frame, double ×tamp) { vpColVector velocity; getVelocity(frame, velocity, timestamp); @@ -1575,9 +1522,12 @@ expressed in meters, while joint rotations X,A,B in radians. 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); +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 @@ -1605,10 +1555,8 @@ bool vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q) while (std::getline(fd, line)) { lineNum++; if (lineNum == 1) { - if (!(line.compare(0, id.size(), id) == - 0)) { // check if Afma4 position file - std::cout << "Error: this position file " << filename - << " is not for Afma4 robot" << std::endl; + if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma4 position file + std::cout << "Error: this position file " << filename << " is not for Afma4 robot" << std::endl; return false; } } @@ -1617,8 +1565,7 @@ bool vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q) } if ((line.compare(0, key.size(), key) == 0)) { // decode position // check if there are at least njoint values in the line - std::vector chain = - vpIoTools::splitChain(line, std::string(" ")); + std::vector chain = vpIoTools::splitChain(line, std::string(" ")); if (chain.size() < njoint + 1) // try to split with tab separator chain = vpIoTools::splitChain(line, std::string("\t")); if (chain.size() < njoint + 1) @@ -1642,8 +1589,7 @@ bool vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q) fd.close(); if (!pos_found) { - std::cout << "Error: unable to find a position for Afma4 robot in " - << filename << std::endl; + std::cout << "Error: unable to find a position for Afma4 robot in " << filename << std::endl; return false; } @@ -1673,8 +1619,7 @@ bool vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q) \sa readPosFile() */ -bool vpRobotAfma4::savePosFile(const std::string &filename, - const vpColVector &q) +bool vpRobotAfma4::savePosFile(const std::string &filename, const vpColVector &q) { FILE *fd; @@ -1693,8 +1638,7 @@ bool vpRobotAfma4::savePosFile(const std::string &filename, #\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])); + 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); @@ -1738,8 +1682,7 @@ void vpRobotAfma4::move(const char *filename) Euler Rxyz representation. */ -void vpRobotAfma4::getDisplacement(vpRobot::vpControlFrameType frame, - vpColVector &displacement) +void vpRobotAfma4::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement) { displacement.resize(6); displacement = 0; @@ -1788,8 +1731,7 @@ void vpRobotAfma4::getDisplacement(vpRobot::vpControlFrameType frame, CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get velocity."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get velocity."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity."); } } diff --git a/modules/robot/src/real-robot/afma4/vpServolens.cpp b/modules/robot/src/real-robot/afma4/vpServolens.cpp index b0b3310819..de583d91c3 100644 --- a/modules/robot/src/real-robot/afma4/vpServolens.cpp +++ b/modules/robot/src/real-robot/afma4/vpServolens.cpp @@ -46,8 +46,7 @@ */ -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #include #include @@ -79,10 +78,7 @@ vpServolens::vpServolens() : remfd(0), isinit(false) {} \sa open() */ -vpServolens::vpServolens(const char *port) : remfd(0), isinit(false) -{ - this->open(port); -} +vpServolens::vpServolens(const char *port) : remfd(0), isinit(false) { this->open(port); } /*! Destructor. @@ -116,16 +112,14 @@ void vpServolens::open(const char *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."); + 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"); + throw vpRobotException(vpRobotException::communicationError, "Error using TCGETS in ioctl"); } // @@ -155,8 +149,7 @@ void vpServolens::open(const char *port) 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"); + throw vpRobotException(vpRobotException::communicationError, "Error using TCGETS in ioctl"); } // Supprime tous les caracteres recus mais non encore lus par read() @@ -172,9 +165,8 @@ void vpServolens::open(const char *port) 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."); + throw vpRobotException(vpRobotException::communicationError, "Cannot dial with the servolens. Check if the " + "serial link is connected."); } } } @@ -202,8 +194,7 @@ void vpServolens::reset() const { if (!isinit) { vpERROR_TRACE("Cannot dial with Servolens."); - throw vpRobotException(vpRobotException::communicationError, - "Cannot dial with Servolens."); + throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens."); } char commande[10]; @@ -239,8 +230,7 @@ void vpServolens::init() const { if (!isinit) { vpERROR_TRACE("Cannot dial with Servolens."); - throw vpRobotException(vpRobotException::communicationError, - "Cannot dial with Servolens."); + throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens."); } char commande[10]; @@ -271,8 +261,7 @@ void vpServolens::enablePrompt(bool active) const { if (!isinit) { vpERROR_TRACE("Cannot dial with Servolens."); - throw vpRobotException(vpRobotException::communicationError, - "Cannot dial with Servolens."); + throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens."); } char commande[10]; @@ -297,8 +286,7 @@ void vpServolens::setController(vpControllerType controller) const { if (!isinit) { vpERROR_TRACE("Cannot dial with Servolens."); - throw vpRobotException(vpRobotException::communicationError, - "Cannot dial with Servolens."); + throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens."); } char commande[10]; @@ -336,8 +324,7 @@ void vpServolens::setAutoIris(bool enable) const { if (!isinit) { vpERROR_TRACE("Cannot dial with Servolens."); - throw vpRobotException(vpRobotException::communicationError, - "Cannot dial with Servolens."); + throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens."); } char commande[10]; @@ -363,8 +350,7 @@ 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."); + throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens."); } char commande[10]; @@ -449,14 +435,13 @@ 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."); + 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 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 */ @@ -564,32 +549,28 @@ pixel size (7.4 um) and from the servolens zoom position. servolens. \code - #include +#include - int main() - { -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || -(defined(__APPLE__) && defined(__MACH__))) // UNIX vpServolens -servolens("/dev/ttyS0"); +int main() +{ + // UNIX vpServolens servolens("/dev/ttyS0"); +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) - vpImage I(240, 320); - vpCameraParameters cam = servolens.getCameraParameters(I); - std::cout << "Camera parameters: " << cam << std::endl; + vpImage 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 + \exception vpRobotException::communicationError : If cannot dial with Servolens. + */ -vpCameraParameters -vpServolens::getCameraParameters(vpImage &I) const +vpCameraParameters vpServolens::getCameraParameters(vpImage &I) const { if (!isinit) { vpERROR_TRACE("Cannot dial with Servolens."); - throw vpRobotException(vpRobotException::communicationError, - "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 @@ -613,11 +594,9 @@ vpServolens::getCameraParameters(vpImage &I) const // 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. + 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); @@ -637,23 +616,20 @@ char vpServolens::wait() const { if (!isinit) { vpERROR_TRACE("Cannot dial with Servolens."); - throw vpRobotException(vpRobotException::communicationError, - "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."); + throw vpRobotException(vpRobotException::communicationError, "Cannot write on Servolens."); } char c; do { r = ::read(this->remfd, &c, 1); c &= 0x7f; if (r != 1) { - throw vpRobotException(vpRobotException::communicationError, - "Cannot read on Servolens."); + throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens."); } } while (c != '>'); return c; @@ -673,8 +649,7 @@ void vpServolens::wait(vpServoType servo) const { if (!isinit) { vpERROR_TRACE("Cannot dial with Servolens."); - throw vpRobotException(vpRobotException::communicationError, - "Cannot dial with Servolens."); + throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens."); } char c; @@ -698,8 +673,7 @@ void vpServolens::wait(vpServoType servo) const do { /* lecture des caracteres */ if (::read(this->remfd, &c, 1) != 1) { - throw vpRobotException(vpRobotException::communicationError, - "Cannot read on Servolens."); + throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens."); } c &= 0x7f; @@ -707,8 +681,7 @@ void vpServolens::wait(vpServoType servo) const if (c == fin_mvt[0]) { /* lecture du caractere suivant */ if (::read(this->remfd, &c, 1) != 1) { - throw vpRobotException(vpRobotException::communicationError, - "Cannot read on Servolens."); + throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens."); } c &= 0x7f; @@ -736,18 +709,16 @@ 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."); + throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens."); } - fd_set readfds; /* list of fds for select to listen to */ + 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(this->remfd), &readfds); - if (select(FD_SETSIZE, &readfds, (fd_set *)NULL, (fd_set *)NULL, &timeout) > - 0) { + if (select(FD_SETSIZE, &readfds, (fd_set *)NULL, (fd_set *)NULL, &timeout) > 0) { ssize_t n = ::read(this->remfd, c, 1); /* read one character at a time */ if (n != 1) return false; @@ -771,16 +742,14 @@ void vpServolens::write(const char *s) const { if (!isinit) { vpERROR_TRACE("Cannot dial with Servolens."); - throw vpRobotException(vpRobotException::communicationError, - "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."); + throw vpRobotException(vpRobotException::communicationError, "Cannot write on Servolens."); } /* diff --git a/modules/robot/src/real-robot/afma6/vpAfma6.cpp b/modules/robot/src/real-robot/afma6/vpAfma6.cpp index 49fb365c9e..dae8a8d093 100644 --- a/modules/robot/src/real-robot/afma6/vpAfma6.cpp +++ b/modules/robot/src/real-robot/afma6/vpAfma6.cpp @@ -61,56 +61,44 @@ /* --- STATIC ------------------------------------------------------------ */ /* ---------------------------------------------------------------------- */ -static const char *opt_Afma6[] = {"JOINT_MAX", "JOINT_MIN", "LONG_56", - "COUPL_56", "CAMERA", "eMc_ROT_XYZ", - "eMc_TRANS_XYZ", NULL}; +static const char *opt_Afma6[] = {"JOINT_MAX", "JOINT_MIN", "LONG_56", "COUPL_56", + "CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", NULL}; #ifdef VISP_HAVE_AFMA6_DATA const std::string vpAfma6::CONST_AFMA6_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + - std::string("/include/const_Afma6.cnf"); + std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + - std::string("/include/const_eMc_ccmop_without_distortion_Afma6.cnf"); + std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_without_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + - std::string("/include/const_eMc_ccmop_with_distortion_Afma6.cnf"); + std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_with_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + - std::string("/include/const_eMc_gripper_without_distortion_Afma6.cnf"); + std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_without_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + - std::string("/include/const_eMc_gripper_with_distortion_Afma6.cnf"); + std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_with_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + - std::string("/include/const_eMc_vacuum_without_distortion_Afma6.cnf"); + std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_without_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + - std::string("/include/const_eMc_vacuum_with_distortion_Afma6.cnf"); + std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_with_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + - std::string("/include/const_eMc_generic_without_distortion_Afma6.cnf"); + std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + - std::string("/include/const_eMc_generic_with_distortion_Afma6.cnf"); + std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_CAMERA_AFMA6_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + - std::string("/include/const_camera_Afma6.xml"); + std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_camera_Afma6.xml"); #endif // VISP_HAVE_AFMA6_DATA 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_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"; @@ -124,8 +112,7 @@ const unsigned int vpAfma6::njoint = 6; */ vpAfma6::vpAfma6() - : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(), - tool_current(vpAfma6::defaultTool), + : _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 are not available. @@ -181,8 +168,7 @@ void vpAfma6::init(void) extrinsic parameters. */ -void vpAfma6::init(const std::string &camera_extrinsic_parameters, - const std::string &camera_intrinsic_parameters) +void vpAfma6::init(const std::string &camera_extrinsic_parameters, const std::string &camera_intrinsic_parameters) { this->parseConfigFile(camera_extrinsic_parameters); @@ -254,8 +240,7 @@ void vpAfma6::init(const std::string &camera_extrinsic_parameters) init(vpAfma6::vpAfma6ToolType, const std::string&) */ -void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, - const vpHomogeneousMatrix &eMc) +void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const vpHomogeneousMatrix &eMc) { this->setToolType(tool); this->set_eMc(eMc); @@ -278,8 +263,7 @@ void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, \param proj_model : Projection model of the camera. */ -void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, - vpCameraParameters::vpCameraParametersProjType proj_model) +void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType proj_model) { this->projModel = proj_model; @@ -545,8 +529,7 @@ int main() \sa getForwardKinematics() */ -int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, - vpColVector &q, const bool &nearest, +int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest, const bool &verbose) const { vpHomogeneousMatrix fMe; @@ -643,15 +626,11 @@ int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, 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; + 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; + 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; } @@ -838,10 +817,7 @@ void vpAfma6::get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const end-effector frame. */ -void vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const -{ - cMe = this->_eMc.inverse(); -} +void vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); } /*! Get the geometric transformation between the end-effector frame and the @@ -1054,8 +1030,7 @@ void vpAfma6::parseConfigFile(const std::string &filename) std::ifstream fdconfig(filename.c_str(), std::ios::in); if (!fdconfig.is_open()) { - throw vpRobotException(vpRobotException::readingParametersError, - "Impossible to read the config file: %s", + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s", filename.c_str()); } @@ -1067,8 +1042,7 @@ void vpAfma6::parseConfigFile(const std::string &filename) while (std::getline(fdconfig, line)) { lineNum++; - if ((line.compare(0, 1, "#") == 0) || - line.empty()) { // skip comment or empty line + if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line continue; } std::istringstream ss(line); @@ -1083,15 +1057,13 @@ void vpAfma6::parseConfigFile(const std::string &filename) switch (code) { case 0: - ss >> this->_joint_max[0] >> this->_joint_max[1] >> - this->_joint_max[2] >> this->_joint_max[3] >> this->_joint_max[4] >> - this->_joint_max[5]; + ss >> 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: - ss >> this->_joint_min[0] >> this->_joint_min[1] >> - this->_joint_min[2] >> this->_joint_min[3] >> this->_joint_min[4] >> - this->_joint_min[5]; + ss >> 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: @@ -1119,8 +1091,7 @@ void vpAfma6::parseConfigFile(const std::string &filename) break; default: - throw(vpRobotException(vpRobotException::readingParametersError, - "Bad configuration file %s line #%d", + throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d", filename.c_str(), lineNum)); } } @@ -1200,13 +1171,12 @@ int main() // 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_AFMA6_DATA and VISP_HAVE_XML2 macros are defined in -vpConfig.h file try { robot.getCameraParameters (cam, I.getWidth(), -I.getHeight()); + // if VISP_HAVE_AFMA6_DATA 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 << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl; } std::cout << "Camera parameters: " << cam << std::endl; #endif @@ -1217,65 +1187,45 @@ I.getHeight()); parameters are not found. */ -void vpAfma6::getCameraParameters(vpCameraParameters &cam, - const unsigned int &image_width, +void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const { #if defined(VISP_HAVE_XML2) && defined(VISP_HAVE_AFMA6_DATA) 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."); + 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."); + 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."); + 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."); + 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; } @@ -1289,8 +1239,7 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, // "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."); + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); } } #else @@ -1299,88 +1248,80 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, 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; + 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); + 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."); + 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; + 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); + 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."); + 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; + 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); + 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."); + 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; + 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); + 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."); + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); } break; } @@ -1423,8 +1364,7 @@ int main() robot.getCameraParameters (cam, I); } catch(...) { - std::cout << "Cannot get camera parameters for image: " << I.getWidth() << -" x " << I.getHeight() << std::endl; + std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl; } std::cout << "Camera parameters: " << cam << std::endl; #endif @@ -1435,8 +1375,7 @@ int main() parameters are not found. */ -void vpAfma6::getCameraParameters(vpCameraParameters &cam, - const vpImage &I) const +void vpAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage &I) const { getCameraParameters(cam, I.getWidth(), I.getHeight()); } @@ -1472,8 +1411,7 @@ int main() robot.getCameraParameters (cam, I); } catch(...) { - std::cout << "Cannot get camera parameters for image: " << I.getWidth() << -" x " << I.getHeight() << std::endl; + std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl; } std::cout << "Camera parameters: " << cam << std::endl; #endif @@ -1485,8 +1423,7 @@ parameters are not found. */ -void vpAfma6::getCameraParameters(vpCameraParameters &cam, - const vpImage &I) const +void vpAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage &I) const { getCameraParameters(cam, I.getWidth(), I.getHeight()); } @@ -1507,16 +1444,12 @@ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6) 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 + << "\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 + << "\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 @@ -1525,13 +1458,11 @@ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6) << "\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; + << "\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 index 874cf8ec0b..29935016a4 100644 --- a/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp +++ b/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp @@ -82,8 +82,7 @@ const double vpRobotAfma6::defaultPositioningVelocity = 15.0; */ void emergencyStopAfma6(int signo) { - std::cout << "Stop the Afma6 application by signal (" << signo - << "): " << (char)7; + std::cout << "Stop the Afma6 application by signal (" << signo << "): " << (char)7; switch (signo) { case SIGINT: std::cout << "SIGINT (stop by ^C) " << std::endl; @@ -254,8 +253,7 @@ void vpRobotAfma6::init(void) // Look if the power is on or off UInt32 HIPowerStatus; UInt32 EStopStatus; - Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); // Print the robot status @@ -273,8 +271,7 @@ void vpRobotAfma6::init(void) 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 << "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 } @@ -306,8 +303,7 @@ void vpRobotAfma6::init(void) ShutDownConnection(); std::cout << "Cannot open connection with the motionblox..." << std::endl; - throw vpRobotException(vpRobotException::constructionError, - "Cannot open connection with the motionblox"); + throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox"); } return; } @@ -348,9 +344,7 @@ void vpRobotAfma6::init(void) \sa vpCameraParameters, init() */ -void vpRobotAfma6::init( - vpAfma6::vpAfma6ToolType tool, - vpCameraParameters::vpCameraParametersProjType projModel) +void vpRobotAfma6::init(vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType projModel) { InitTry; // Read the robot constants from files @@ -447,8 +441,7 @@ int main() vpCameraParameters::vpCameraParametersProjType), init(vpAfma6::vpAfma6ToolType, const std::string&) */ -void vpRobotAfma6::init(vpAfma6::vpAfma6ToolType tool, - const vpHomogeneousMatrix &eMc) +void vpRobotAfma6::init(vpAfma6::vpAfma6ToolType tool, const vpHomogeneousMatrix &eMc) { InitTry; // Read the robot constants from files @@ -526,8 +519,7 @@ eMc_TRANS_XYZ 0.05 0.01 0.06 vpCameraParameters::vpCameraParametersProjType), init(vpAfma6::vpAfma6ToolType, const vpHomogeneousMatrix&) */ -void vpRobotAfma6::init(vpAfma6::vpAfma6ToolType tool, - const std::string &filename) +void vpRobotAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename) { InitTry; // Read the robot constants from files @@ -573,8 +565,7 @@ vpRobotAfma6::~vpRobotAfma6(void) // Look if the power is on or off UInt32 HIPowerStatus; - Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); // if (HIPowerStatus == 1) { @@ -599,8 +590,7 @@ Change the robot state. \param newState : New requested robot state. */ -vpRobot::vpRobotStateType -vpRobotAfma6::setRobotState(vpRobot::vpRobotStateType newState) +vpRobot::vpRobotStateType vpRobotAfma6::setRobotState(vpRobot::vpRobotStateType newState) { InitTry; @@ -613,8 +603,7 @@ vpRobotAfma6::setRobotState(vpRobot::vpRobotStateType newState) } case vpRobot::STATE_POSITION_CONTROL: { if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) { - std::cout - << "Change the control mode from velocity to position control.\n"; + 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 @@ -659,8 +648,7 @@ void vpRobotAfma6::stopMotion(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot stop robot motion"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot stop robot motion."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion."); } } @@ -684,8 +672,7 @@ void vpRobotAfma6::powerOn(void) unsigned int nitermax = 10; for (unsigned int i = 0; i < nitermax; i++) { - Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus)); if (EStopStatus == ESTOP_AUTO) { break; // exit for loop } else if (EStopStatus == ESTOP_MANUAL) { @@ -702,8 +689,7 @@ void vpRobotAfma6::powerOn(void) fflush(stdout); CAL_Wait(1); } else { - std::cout << "Sorry there is an error on the emergency chain." - << std::endl; + 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(); @@ -716,8 +702,7 @@ void vpRobotAfma6::powerOn(void) if (EStopStatus == ESTOP_ACTIVATED) { std::cout << "Sorry, cannot power on the robot." << std::endl; - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot power on the robot."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."); } if (HIPowerStatus == 0) { @@ -730,8 +715,7 @@ void vpRobotAfma6::powerOn(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot power on the robot"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot power off the robot."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."); } } @@ -750,8 +734,7 @@ void vpRobotAfma6::powerOff(void) // Look if the power is on or off UInt32 HIPowerStatus; - Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); if (HIPowerStatus == 1) { @@ -764,8 +747,7 @@ void vpRobotAfma6::powerOff(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot power off the robot"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot power off the robot."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."); } } @@ -786,8 +768,7 @@ bool vpRobotAfma6::getPowerState(void) bool status = false; // Look if the power is on or off UInt32 HIPowerStatus; - Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); if (HIPowerStatus == 1) { @@ -797,8 +778,7 @@ bool vpRobotAfma6::getPowerState(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get the power status"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get the power status."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status."); } return status; } @@ -830,10 +810,7 @@ void vpRobotAfma6::get_cVe(vpVelocityTwistMatrix &cVe) const end-effector frame. */ -void vpRobotAfma6::get_cMe(vpHomogeneousMatrix &cMe) const -{ - vpAfma6::get_cMe(cMe); -} +void vpRobotAfma6::get_cMe(vpHomogeneousMatrix &cMe) const { vpAfma6::get_cMe(cMe); } /*! @@ -939,20 +916,14 @@ void vpRobotAfma6::get_fJe(vpMatrix &fJe) \sa getPositioningVelocity() */ -void vpRobotAfma6::setPositioningVelocity(const double velocity) -{ - positioningVelocity = velocity; -} +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; -} +double vpRobotAfma6::getPositioningVelocity(void) { return positioningVelocity; } /*! @@ -1029,8 +1000,7 @@ int main() \endcode */ -void vpRobotAfma6::setPosition(const vpRobot::vpControlFrameType frame, - const vpPoseVector &pose) +void vpRobotAfma6::setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose) { vpColVector position(6); @@ -1045,10 +1015,8 @@ void vpRobotAfma6::setPosition(const vpRobot::vpControlFrameType frame, position[i + 3] = rxyz[i]; } if (frame == vpRobot::ARTICULAR_FRAME) { - throw vpRobotException( - vpRobotException::lowLevelError, - "Positionning error: " - "Joint frame not implemented for pose positionning."); + throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: " + "Joint frame not implemented for pose positionning."); } setPosition(frame, position); } @@ -1135,8 +1103,7 @@ int main() */ -void vpRobotAfma6::setPosition(const vpRobot::vpControlFrameType frame, - const vpColVector &position) +void vpRobotAfma6::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) { if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) { @@ -1233,9 +1200,8 @@ void vpRobotAfma6::setPosition(const vpRobot::vpControlFrameType frame, } case vpRobot::MIXT_FRAME: { vpERROR_TRACE("Positionning error. Mixt frame not implemented"); - throw vpRobotException(vpRobotException::lowLevelError, - "Positionning error: " - "Mixt frame not implemented."); + throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: " + "Mixt frame not implemented."); } } @@ -1249,8 +1215,7 @@ void vpRobotAfma6::setPosition(const vpRobot::vpControlFrameType frame, if (TryStt < 0 || error < 0) { vpERROR_TRACE("Positionning error."); - throw vpRobotException(vpRobotException::positionOutOfRangeError, - "Position out of range."); + throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range."); } return; @@ -1314,8 +1279,7 @@ int main() robot.setPositioningVelocity(20); // Moves the robot in the camera frame - robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, -pos6); + robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6); return 0; #endif @@ -1324,10 +1288,8 @@ pos6); \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) +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); @@ -1395,8 +1357,7 @@ void vpRobotAfma6::setPosition(const std::string &filename) if (ret == false) { vpERROR_TRACE("Bad position in \"%s\"", filename.c_str()); - throw vpRobotException(vpRobotException::lowLevelError, - "Bad position in filename."); + throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename."); } this->setRobotState(vpRobot::STATE_POSITION_CONTROL); this->setPosition(vpRobot::ARTICULAR_FRAME, q); @@ -1456,8 +1417,7 @@ void vpRobotAfma6::setPosition(const std::string &filename) vpColVector & r) */ -void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &position, double ×tamp) +void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double ×tamp) { InitTry; @@ -1504,17 +1464,15 @@ void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, } 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"); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: " + "not implemented"); } } CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get position."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get position."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position."); } return; @@ -1540,8 +1498,7 @@ double vpRobotAfma6::getTime() const The difference is here that the timestamp is not used. */ -void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &position) +void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) { double timestamp; getPosition(frame, position, timestamp); @@ -1556,8 +1513,7 @@ void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, representation. */ -void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, - vpPoseVector &position, double ×tamp) +void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp) { vpColVector posRxyz; // recupere position en Rxyz @@ -1585,8 +1541,7 @@ void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, The difference is here that the timestamp is not used. */ -void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, - vpPoseVector &position) +void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position) { double timestamp; getPosition(frame, position, timestamp); @@ -1655,17 +1610,14 @@ void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot); \endcode */ -void vpRobotAfma6::setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &vel) +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) "); + 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); @@ -1711,8 +1663,7 @@ void vpRobotAfma6::setVelocity(const vpRobot::vpControlFrameType frame, 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; + std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl; } } else { printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt); @@ -1781,8 +1732,7 @@ void vpRobotAfma6::setVelocity(const vpRobot::vpControlFrameType frame, } \endcode */ -void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &velocity, double ×tamp) +void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double ×tamp) { velocity.resize(6); velocity = 0; @@ -1876,8 +1826,7 @@ void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get velocity."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get velocity."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity."); } } @@ -1889,8 +1838,7 @@ void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &, double &) except that the timestamp is not returned. */ -void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &velocity) +void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity) { double timestamp; getVelocity(frame, velocity, timestamp); @@ -1937,8 +1885,7 @@ void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, } \endcode */ -vpColVector vpRobotAfma6::getVelocity(vpRobot::vpControlFrameType frame, - double ×tamp) +vpColVector vpRobotAfma6::getVelocity(vpRobot::vpControlFrameType frame, double ×tamp) { vpColVector velocity; getVelocity(frame, velocity, timestamp); @@ -1998,9 +1945,12 @@ expressed in meters, while joint rotations A,B,C in radians. 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); +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 @@ -2028,10 +1978,8 @@ bool vpRobotAfma6::readPosFile(const std::string &filename, vpColVector &q) while (std::getline(fd, line)) { lineNum++; if (lineNum == 1) { - if (!(line.compare(0, id.size(), id) == - 0)) { // check if Afma6 position file - std::cout << "Error: this position file " << filename - << " is not for Afma6 robot" << std::endl; + if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file + std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl; return false; } } @@ -2040,8 +1988,7 @@ bool vpRobotAfma6::readPosFile(const std::string &filename, vpColVector &q) } if ((line.compare(0, key.size(), key) == 0)) { // decode position // check if there are at least njoint values in the line - std::vector chain = - vpIoTools::splitChain(line, std::string(" ")); + std::vector chain = vpIoTools::splitChain(line, std::string(" ")); if (chain.size() < njoint + 1) // try to split with tab separator chain = vpIoTools::splitChain(line, std::string("\t")); if (chain.size() < njoint + 1) @@ -2065,8 +2012,7 @@ bool vpRobotAfma6::readPosFile(const std::string &filename, vpColVector &q) fd.close(); if (!pos_found) { - std::cout << "Error: unable to find a position for Afma6 robot in " - << filename << std::endl; + std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl; return false; } @@ -2097,8 +2043,7 @@ bool vpRobotAfma6::readPosFile(const std::string &filename, vpColVector &q) \sa readPosFile() */ -bool vpRobotAfma6::savePosFile(const std::string &filename, - const vpColVector &q) +bool vpRobotAfma6::savePosFile(const std::string &filename, const vpColVector &q) { FILE *fd; @@ -2116,8 +2061,8 @@ bool vpRobotAfma6::savePosFile(const std::string &filename, #\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])); + 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); @@ -2179,8 +2124,7 @@ void vpRobotAfma6::openGripper() CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot open the gripper"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot open the gripper."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper."); } } @@ -2199,8 +2143,7 @@ void vpRobotAfma6::closeGripper() CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot close the gripper"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot close the gripper."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper."); } } @@ -2222,8 +2165,7 @@ void vpRobotAfma6::closeGripper() Euler Rxyz representation. */ -void vpRobotAfma6::getDisplacement(vpRobot::vpControlFrameType frame, - vpColVector &displacement) +void vpRobotAfma6::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement) { displacement.resize(6); displacement = 0; @@ -2293,8 +2235,7 @@ void vpRobotAfma6::getDisplacement(vpRobot::vpControlFrameType frame, CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get velocity."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get velocity."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity."); } } @@ -2318,8 +2259,7 @@ bool vpRobotAfma6::checkJointLimits(vpColVector &jointsStatus) 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; + std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl; jointsStatus[i] = axisInJoint[i]; status = false; } else { @@ -2330,8 +2270,7 @@ bool vpRobotAfma6::checkJointLimits(vpColVector &jointsStatus) Catch(); if (TryStt < 0) { vpERROR_TRACE("Cannot check joint limits."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot check joint limits."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot check joint limits."); } return status; diff --git a/modules/robot/src/real-robot/biclops/vpBiclops.cpp b/modules/robot/src/real-robot/biclops/vpBiclops.cpp index 95c565e9ec..910f9aad4a 100644 --- a/modules/robot/src/real-robot/biclops/vpBiclops.cpp +++ b/modules/robot/src/real-robot/biclops/vpBiclops.cpp @@ -49,20 +49,14 @@ /* ------------------------------------------------------------------------ */ /* --- COMPUTE ------------------------------------------------------------ */ /* ------------------------------------------------------------------------ */ -const unsigned int vpBiclops::ndof = - 2; /*configfile = filename; -} +void vpRobotBiclops::setConfigFile(const std::string &filename) { this->configfile = filename; } /*! @@ -252,10 +245,8 @@ void vpRobotBiclops::init() // test if the config file exists FILE *fd = fopen(configfile.c_str(), "r"); if (fd == NULL) { - vpCERROR << "Cannot open biclops config file: " << configfile - << std::endl; - throw vpRobotException(vpRobotException::constructionError, - "Cannot open connection with biclops"); + vpCERROR << "Cannot open biclops config file: " << configfile << std::endl; + throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops"); } fclose(fd); @@ -300,8 +291,7 @@ void vpRobotBiclops::init() */ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) { - vpRobotBiclopsController *controller = - static_cast(arg); + vpRobotBiclopsController *controller = static_cast(arg); int iter = 0; // PMDAxisControl *panAxis = controller->getPanAxis(); @@ -318,13 +308,12 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) 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 + 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])); + 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"); @@ -382,14 +371,10 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) 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])); + 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 @@ -409,8 +394,7 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) // 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::epsilon()) + std::fabs(vpMath::maximum(shm.q_dot[i], prev_q_dot[i])) * std::numeric_limits::epsilon()) new_q_dot[i] = true; else new_q_dot[i] = false; @@ -422,13 +406,11 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) 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, "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]); + 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++) { @@ -470,10 +452,9 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) // 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 + 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 } } } @@ -490,7 +471,7 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) // 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 + if (enable_limit[i] == true) { // limit detection active // Test if this axis was stopped before if (force_halt[i] == false) { @@ -502,8 +483,7 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) } } else { // No need to stop the robot - enable_limit[i] = - true; // Normal situation, activate limit detection + enable_limit[i] = true; // Normal situation, activate limit detection } } } @@ -514,8 +494,7 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) 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])); + 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); @@ -567,8 +546,7 @@ void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg) speed control. */ -vpRobot::vpRobotStateType -vpRobotBiclops::setRobotState(vpRobot::vpRobotStateType newState) +vpRobot::vpRobotStateType vpRobotBiclops::setRobotState(vpRobot::vpRobotStateType newState) { switch (newState) { case vpRobot::STATE_STOP: { @@ -593,12 +571,9 @@ vpRobotBiclops::setRobotState(vpRobot::vpRobotStateType newState) vpDEBUG_TRACE(12, "Create speed control thread"); int code; - code = pthread_create(&control_thread, NULL, - &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, - &controller); + code = pthread_create(&control_thread, NULL, &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, &controller); if (code != 0) { - vpCERROR << "Cannot create speed biclops control thread: " << code - << " strErr=" << strerror(errno) + vpCERROR << "Cannot create speed biclops control thread: " << code << " strErr=" << strerror(errno) << " strCode=" << strerror(code) << std::endl; } @@ -657,10 +632,7 @@ void vpRobotBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const \param cMe : Homogeneous matrix between camera and end effector frame. */ -void vpRobotBiclops::get_cMe(vpHomogeneousMatrix &cMe) const -{ - cMe = vpBiclops::get_cMe(); -} +void vpRobotBiclops::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = vpBiclops::get_cMe(); } /*! Get the robot jacobian expressed in the end-effector frame. @@ -716,8 +688,7 @@ void vpRobotBiclops::setPositioningVelocity(const double velocity) { if (velocity < 0 || velocity > 100) { vpERROR_TRACE("Bad positionning velocity"); - throw vpRobotException(vpRobotException::constructionError, - "Bad positionning velocity"); + throw vpRobotException(vpRobotException::constructionError, "Bad positionning velocity"); } positioningVelocity = velocity; @@ -729,10 +700,7 @@ void vpRobotBiclops::setPositioningVelocity(const double velocity) maximum positionning velocity is given vpBiclops::speedLimit. */ -double vpRobotBiclops::getPositioningVelocity(void) -{ - return positioningVelocity; -} +double vpRobotBiclops::getPositioningVelocity(void) { return positioningVelocity; } /*! Move the robot in position control. @@ -749,8 +717,7 @@ double vpRobotBiclops::getPositioningVelocity(void) type is given. */ -void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, - const vpColVector &q) +void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) { if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) { @@ -763,23 +730,20 @@ void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType 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"); + 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"); + 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: " + "not implemented"); break; case vpRobot::ARTICULAR_FRAME: break; @@ -817,8 +781,7 @@ void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, type is given. */ -void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, - const double &q1, const double &q2) +void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2) { try { vpColVector q(2); @@ -850,8 +813,7 @@ 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"); + throw vpRobotException(vpRobotException::readingParametersError, "Cannot get biclops position from file"); } setPosition(vpRobot::ARTICULAR_FRAME, q); } @@ -871,29 +833,25 @@ void vpRobotBiclops::setPosition(const char *filename) is given. */ -void vpRobotBiclops::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &q) +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"); + 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"); + 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: " + "not implemented"); break; case vpRobot::ARTICULAR_FRAME: break; @@ -914,8 +872,7 @@ void vpRobotBiclops::getPosition(const vpRobot::vpControlFrameType frame, q.resize(vpBiclops::ndof); vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex"); - pthread_mutex_lock( - &vpMeasure_mutex); // Wait until a position is available + pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available vpRobotBiclopsController::shmType shm; @@ -966,17 +923,14 @@ void vpRobotBiclops::getPosition(const vpRobot::vpControlFrameType frame, autorized speed (see vpRobot::maxRotationVelocity). */ -void vpRobotBiclops::setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &q_dot) +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) "); + 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) { @@ -984,17 +938,15 @@ void vpRobotBiclops::setVelocity(const vpRobot::vpControlFrameType 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot " + "in the camera frame:" + "functionality not implemented"); } 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector " + "in articular frame"); } break; } @@ -1002,25 +954,22 @@ void vpRobotBiclops::setVelocity(const vpRobot::vpControlFrameType 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot " + "in the reference frame:" + "functionality not implemented"); } 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot " + "in the mixt frame:" + "functionality not implemented"); } default: { vpERROR_TRACE("Error in spec of vpRobot. " "Case not taken in account."); - throw vpRobotException(vpRobotException::wrongStateError, - "Cannot send a velocity to the robot "); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "); } } @@ -1088,29 +1037,25 @@ void vpRobotBiclops::setVelocity(const vpRobot::vpControlFrameType frame, \exception vpRobotException::wrongStateError : If a not supported frame type is given. */ -void vpRobotBiclops::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &q_dot) +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"); + 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"); + 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: " + "not implemented"); break; case vpRobot::ARTICULAR_FRAME: break; @@ -1131,8 +1076,7 @@ void vpRobotBiclops::getVelocity(const vpRobot::vpControlFrameType frame, q_dot.resize(vpBiclops::ndof); vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex"); - pthread_mutex_lock( - &vpMeasure_mutex); // Wait until a position is available + pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available vpRobotBiclopsController::shmType shm; @@ -1195,8 +1139,7 @@ vpColVector vpRobotBiclops::getVelocity(vpRobot::vpControlFrameType frame) \return true if a position was found, false otherwise. */ -bool vpRobotBiclops::readPositionFile(const std::string &filename, - vpColVector &q) +bool vpRobotBiclops::readPositionFile(const std::string &filename, vpColVector &q) { std::ifstream fd(filename.c_str(), std::ios::in); @@ -1215,10 +1158,8 @@ bool vpRobotBiclops::readPositionFile(const std::string &filename, while (std::getline(fd, line)) { lineNum++; if (lineNum == 1) { - if (!(line.compare(0, id.size(), id) == - 0)) { // check if Biclops position file - std::cout << "Error: this position file " << filename - << " is not for Biclops robot" << std::endl; + if (!(line.compare(0, id.size(), id) == 0)) { // check if Biclops position file + std::cout << "Error: this position file " << filename << " is not for Biclops robot" << std::endl; return false; } } @@ -1227,10 +1168,8 @@ bool vpRobotBiclops::readPositionFile(const std::string &filename, } if ((line.compare(0, key.size(), key) == 0)) { // decode position // check if there are at least njoint values in the line - std::vector chain = - vpIoTools::splitChain(line, std::string(" ")); - if (chain.size() < - vpBiclops::ndof + 1) // try to split with tab separator + std::vector chain = vpIoTools::splitChain(line, std::string(" ")); + if (chain.size() < vpBiclops::ndof + 1) // try to split with tab separator chain = vpIoTools::splitChain(line, std::string("\t")); if (chain.size() < vpBiclops::ndof + 1) continue; @@ -1251,8 +1190,7 @@ bool vpRobotBiclops::readPositionFile(const std::string &filename, fd.close(); if (!pos_found) { - std::cout << "Error: unable to find a position for Biclops robot in " - << filename << std::endl; + std::cout << "Error: unable to find a position for Biclops robot in " << filename << std::endl; return false; } @@ -1281,8 +1219,7 @@ bool vpRobotBiclops::readPositionFile(const std::string &filename, is given. */ -void vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame, - vpColVector &d) +void vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &d) { vpColVector q_current; // current position @@ -1313,16 +1250,14 @@ void vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame, 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"); + 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the mixt frame:" + "functionality not implemented"); break; } diff --git a/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp b/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp index 597101203c..1d0cbf9aac 100644 --- a/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp +++ b/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp @@ -38,8 +38,7 @@ #include #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX #include #endif #include @@ -65,9 +64,8 @@ Default constructor. */ vpRobotBiclopsController::vpRobotBiclopsController() - : biclops(), axisMask(0), panAxis(NULL), tiltAxis(NULL), vergeAxis(NULL), - panProfile(), tiltProfile(), vergeProfile(), shm(), - stopControllerThread_(false) + : 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. @@ -132,12 +130,10 @@ void vpRobotBiclopsController::init(const std::string &configfile) 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 << "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."); + throw vpRobotException(vpRobotException::notInitializedError, "Cannot initialize biclops head."); } vpDEBUG_TRACE(12, "Biclops initialized"); @@ -165,8 +161,7 @@ void vpRobotBiclopsController::init(const std::string &configfile) vpDEBUG_TRACE(12, "Homing sequence succeed."); else { vpERROR_TRACE("Homing sequence failed. Program is stopped"); - throw vpRobotException(vpRobotException::constructionError, - "Cannot open connection with biclops"); + throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops"); } // Get the currently defined (default) motion profiles. @@ -192,13 +187,11 @@ void vpRobotBiclopsController::init(const std::string &configfile) */ -void vpRobotBiclopsController::setPosition(const vpColVector &q, - const double percentVelocity) +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."); + throw vpRobotException(vpRobotException::lowLevelError, "Bad dimension for positioning vector."); } panAxis->SetProfileMode(PMDTrapezoidalProfile); @@ -219,12 +212,10 @@ void vpRobotBiclopsController::setPosition(const vpColVector &q, // 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.); + panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); tiltProfile.pos = PMDUtils::RadsToRevs(q[1]); - tiltProfile.vel = - PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); + tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); // Inform the controller of the new desired position. panAxis->SetProfile(panProfile); @@ -238,11 +229,9 @@ void vpRobotBiclopsController::setPosition(const vpColVector &q, // 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.); + panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); - vpDEBUG_TRACE(12, "Speed percent: %lf", - 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; @@ -254,8 +243,7 @@ void vpRobotBiclopsController::setPosition(const vpColVector &q, // 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.); + tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.); tiltAxis->ProfileToCounts(tiltProfile, desired_profile); vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl; @@ -282,8 +270,7 @@ 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."); + throw vpRobotException(vpRobotException::lowLevelError, "Bad dimension for velocity vector."); } #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API diff --git a/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp b/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp index 8a41a5f891..b5b8457b34 100644 --- a/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp +++ b/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp @@ -98,8 +98,7 @@ vpRobotPioneer::~vpRobotPioneer() dimension vector. \exception vpRobotException::wrongStateError : If the specified control frame is not supported. */ -void vpRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &vel) +void vpRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) { init(); @@ -113,8 +112,7 @@ void vpRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, } */ if (vel.size() != 2) { - throw(vpRobotException(vpRobotException::dimensionError, - "Velocity vector is not a 2 dimension vector")); + throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector")); } vpColVector vel_max(2); @@ -140,9 +138,8 @@ void vpRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, 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"); + throw vpRobotException(vpRobotException::wrongStateError, + "Cannot send the robot velocity in the specified control frame"); } } @@ -189,8 +186,7 @@ void vpRobotPioneer::init() \sa getVelocity(const vpRobot::vpControlFrameType) */ -void vpRobotPioneer::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &velocity) +void vpRobotPioneer::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity) { init(); velocity.resize(2); @@ -206,9 +202,8 @@ void vpRobotPioneer::getVelocity(const vpRobot::vpControlFrameType frame, velocity[1] = vpMath::rad(this->getRotVel()); this->unlock(); } else { - throw vpRobotException( - vpRobotException::wrongStateError, - "Cannot get the robot volocity in the specified control frame"); + throw vpRobotException(vpRobotException::wrongStateError, + "Cannot get the robot volocity in the specified control frame"); } } @@ -232,8 +227,7 @@ void vpRobotPioneer::getVelocity(const vpRobot::vpControlFrameType frame, \sa getVelocity(const vpRobot::vpControlFrameType, vpColVector &) */ -vpColVector -vpRobotPioneer::getVelocity(const vpRobot::vpControlFrameType frame) +vpColVector vpRobotPioneer::getVelocity(const vpRobot::vpControlFrameType frame) { vpColVector velocity; getVelocity(frame, velocity); diff --git a/modules/robot/src/real-robot/ptu46/vpPtu46.cpp b/modules/robot/src/real-robot/ptu46/vpPtu46.cpp index 9aa0eaa295..8b158a2547 100644 --- a/modules/robot/src/real-robot/ptu46/vpPtu46.cpp +++ b/modules/robot/src/real-robot/ptu46/vpPtu46.cpp @@ -52,10 +52,10 @@ /* --- COMPUTE ------------------------------------------------------------ */ /* ------------------------------------------------------------------------ */ const unsigned int vpPtu46::ndof = 2; /*maxRotationVelocity; - bool norm = - false; // Flag to indicate when velocities need to be nomalized + bool norm = false; // Flag to indicate when velocities need to be nomalized for (unsigned int i = 0; i < 2; ++i) // rx and ry of the camera { if (fabs(v[i]) > max) { @@ -599,8 +566,7 @@ void vpRobotPtu46::setVelocity(const vpRobot::vpControlFrameType frame, break; } - vpCDEBUG(12) << "v: " << ptuSpeedInterface[0] << " " << ptuSpeedInterface[1] - << std::endl; + vpCDEBUG(12) << "v: " << ptuSpeedInterface[0] << " " << ptuSpeedInterface[1] << std::endl; ptu.move(ptuSpeedInterface, ptuFrameInterface); return; } @@ -623,8 +589,7 @@ void vpRobotPtu46::setVelocity(const vpRobot::vpControlFrameType frame, \exception vpRobotException::wrongStateError : If a not supported frame type is given. */ -void vpRobotPtu46::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &q_dot) +void vpRobotPtu46::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot) { TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME; @@ -633,9 +598,8 @@ void vpRobotPtu46::getVelocity(const vpRobot::vpControlFrameType 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the camera frame:" + "functionality not implemented"); } case vpRobot::ARTICULAR_FRAME: { ptuFrameInterface = PTU_ARTICULAR_FRAME; @@ -644,17 +608,15 @@ void vpRobotPtu46::getVelocity(const vpRobot::vpControlFrameType frame, 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the reference frame:" + "functionality not implemented"); } 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the mixt frame:" + "functionality not implemented"); } } @@ -705,8 +667,7 @@ vpColVector vpRobotPtu46::getVelocity(vpRobot::vpControlFrameType frame) \return true if a position was found, false otherwise. */ -bool vpRobotPtu46::readPositionFile(const std::string &filename, - vpColVector &q) +bool vpRobotPtu46::readPositionFile(const std::string &filename, vpColVector &q) { std::ifstream fd(filename.c_str(), std::ios::in); @@ -725,10 +686,8 @@ bool vpRobotPtu46::readPositionFile(const std::string &filename, while (std::getline(fd, line)) { lineNum++; if (lineNum == 1) { - if (!(line.compare(0, id.size(), id) == - 0)) { // check if Ptu-46 position file - std::cout << "Error: this position file " << filename - << " is not for Ptu-46 robot" << std::endl; + if (!(line.compare(0, id.size(), id) == 0)) { // check if Ptu-46 position file + std::cout << "Error: this position file " << filename << " is not for Ptu-46 robot" << std::endl; return false; } } @@ -737,8 +696,7 @@ bool vpRobotPtu46::readPositionFile(const std::string &filename, } if ((line.compare(0, key.size(), key) == 0)) { // decode position // check if there are at least njoint values in the line - std::vector chain = - vpIoTools::splitChain(line, std::string(" ")); + std::vector chain = vpIoTools::splitChain(line, std::string(" ")); if (chain.size() < vpPtu46::ndof + 1) // try to split with tab separator chain = vpIoTools::splitChain(line, std::string("\t")); if (chain.size() < vpPtu46::ndof + 1) @@ -760,8 +718,7 @@ bool vpRobotPtu46::readPositionFile(const std::string &filename, fd.close(); if (!pos_found) { - std::cout << "Error: unable to find a position for Ptu-46 robot in " - << filename << std::endl; + std::cout << "Error: unable to find a position for Ptu-46 robot in " << filename << std::endl; return false; } @@ -787,8 +744,7 @@ bool vpRobotPtu46::readPositionFile(const std::string &filename, is given. */ -void vpRobotPtu46::getDisplacement(vpRobot::vpControlFrameType frame, - vpColVector &d) +void vpRobotPtu46::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &d) { double d_[6]; @@ -814,16 +770,14 @@ void vpRobotPtu46::getDisplacement(vpRobot::vpControlFrameType frame, 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the reference frame:" + "functionality not implemented"); } 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the reference frame:" + "functionality not implemented"); } } } diff --git a/modules/robot/src/real-robot/viper/vpRobotViper650.cpp b/modules/robot/src/real-robot/viper/vpRobotViper650.cpp index 91f990e5b1..ac7a6fcb94 100644 --- a/modules/robot/src/real-robot/viper/vpRobotViper650.cpp +++ b/modules/robot/src/real-robot/viper/vpRobotViper650.cpp @@ -82,8 +82,7 @@ const double vpRobotViper650::defaultPositioningVelocity = 15.0; */ void emergencyStopViper650(int signo) { - std::cout << "Stop the Viper650 application by signal (" << signo - << "): " << (char)7; + std::cout << "Stop the Viper650 application by signal (" << signo << "): " << (char)7; switch (signo) { case SIGINT: std::cout << "SIGINT (stop by ^C) " << std::endl; @@ -281,8 +280,7 @@ void vpRobotViper650::init(void) // Look if the power is on or off UInt32 HIPowerStatus; UInt32 EStopStatus; - Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); // Print the robot status @@ -309,8 +307,7 @@ void vpRobotViper650::init(void) 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 << "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 } @@ -346,8 +343,7 @@ void vpRobotViper650::init(void) ShutDownConnection(); std::cout << "Cannot open connection with the motionblox..." << std::endl; - throw vpRobotException(vpRobotException::constructionError, - "Cannot open connection with the motionblox"); + throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox"); } return; } @@ -383,8 +379,7 @@ int main() // Set the extrinsic camera parameters obtained with a perpective // projection model including a distorsion parameter - robot.init(vpViper650::TOOL_MARLIN_F033C_CAMERA, - vpCameraParameters::perspectiveProjWithDistortion); + robot.init(vpViper650::TOOL_MARLIN_F033C_CAMERA, vpCameraParameters::perspectiveProjWithDistortion); \endcode Now, you can get the intrinsic camera parameters associated to an @@ -410,9 +405,7 @@ int main() init(vpViper650::vpToolType, const vpHomogeneousMatrix&), init(vpViper650::vpToolType, const std::string&) */ -void vpRobotViper650::init( - vpViper650::vpToolType tool, - vpCameraParameters::vpCameraParametersProjType projModel) +void vpRobotViper650::init(vpViper650::vpToolType tool, vpCameraParameters::vpCameraParametersProjType projModel) { // Read the robot constants from files // - joint [min,max], coupl_56, long_56 @@ -495,8 +488,7 @@ eMc_TRANS_XYZ 0.05 0.01 0.06 vpCameraParameters::vpCameraParametersProjType), init(vpViper650::vpToolType, const vpHomogeneousMatrix&) */ -void vpRobotViper650::init(vpViper650::vpToolType tool, - const std::string &filename) +void vpRobotViper650::init(vpViper650::vpToolType tool, const std::string &filename) { vpViper650::init(tool, filename); @@ -562,8 +554,7 @@ int main() vpCameraParameters::vpCameraParametersProjType), init(vpViper650::vpToolType, const std::string&) */ -void vpRobotViper650::init(vpViper650::vpToolType tool, - const vpHomogeneousMatrix &eMc_) +void vpRobotViper650::init(vpViper650::vpToolType tool, const vpHomogeneousMatrix &eMc_) { vpViper650::init(tool, eMc_); @@ -636,8 +627,7 @@ void vpRobotViper650::set_eMc(const vpHomogeneousMatrix &eMc_) \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_) +void vpRobotViper650::set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_) { this->vpViper650::set_eMc(etc_, erc_); @@ -675,8 +665,7 @@ vpRobotViper650::~vpRobotViper650(void) // Look if the power is on or off UInt32 HIPowerStatus; - Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); // if (HIPowerStatus == 1) { @@ -701,8 +690,7 @@ Change the robot state. \param newState : New requested robot state. */ -vpRobot::vpRobotStateType -vpRobotViper650::setRobotState(vpRobot::vpRobotStateType newState) +vpRobot::vpRobotStateType vpRobotViper650::setRobotState(vpRobot::vpRobotStateType newState) { InitTry; @@ -711,15 +699,13 @@ vpRobotViper650::setRobotState(vpRobot::vpRobotStateType newState) // Start primitive STOP only if the current state is Velocity if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) { Try(PrimitiveSTOP_Viper650()); - vpTime::sleepMs( - 100); // needed to ensure velocity task ends up on low level + vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level } break; } case vpRobot::STATE_POSITION_CONTROL: { if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) { - std::cout - << "Change the control mode from velocity to position control.\n"; + 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 @@ -767,8 +753,7 @@ void vpRobotViper650::stopMotion(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot stop robot motion"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot stop robot motion."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion."); } } @@ -792,8 +777,7 @@ void vpRobotViper650::powerOn(void) unsigned int nitermax = 10; for (unsigned int i = 0; i < nitermax; i++) { - Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus)); if (EStopStatus == ESTOP_AUTO) { controlMode = AUTO; break; // exit for loop @@ -813,8 +797,7 @@ void vpRobotViper650::powerOn(void) fflush(stdout); CAL_Wait(1); } else { - std::cout << "Sorry there is an error on the emergency chain." - << std::endl; + 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(); @@ -827,8 +810,7 @@ void vpRobotViper650::powerOn(void) if (EStopStatus == ESTOP_ACTIVATED) { std::cout << "Sorry, cannot power on the robot." << std::endl; - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot power on the robot."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."); } if (HIPowerStatus == 0) { @@ -841,8 +823,7 @@ void vpRobotViper650::powerOn(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot power on the robot"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot power off the robot."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."); } } @@ -861,8 +842,7 @@ void vpRobotViper650::powerOff(void) // Look if the power is on or off UInt32 HIPowerStatus; - Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); if (HIPowerStatus == 1) { @@ -875,8 +855,7 @@ void vpRobotViper650::powerOff(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot power off the robot"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot power off the robot."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."); } } @@ -897,8 +876,7 @@ bool vpRobotViper650::getPowerState(void) const bool status = false; // Look if the power is on or off UInt32 HIPowerStatus; - Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); if (HIPowerStatus == 1) { @@ -908,8 +886,7 @@ bool vpRobotViper650::getPowerState(void) const CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get the power status"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get the power status."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status."); } return status; } @@ -942,10 +919,7 @@ void vpRobotViper650::get_cVe(vpVelocityTwistMatrix &cVe) const end-effector frame. */ -void vpRobotViper650::get_cMe(vpHomogeneousMatrix &cMe) const -{ - vpViper650::get_cMe(cMe); -} +void vpRobotViper650::get_cMe(vpHomogeneousMatrix &cMe) const { vpViper650::get_cMe(cMe); } /*! @@ -1050,20 +1024,14 @@ int main() \sa getPositioningVelocity() */ -void vpRobotViper650::setPositioningVelocity(const double velocity) -{ - positioningVelocity = velocity; -} +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; -} +double vpRobotViper650::getPositioningVelocity(void) const { return positioningVelocity; } /*! @@ -1143,8 +1111,7 @@ int main() \endcode */ -void vpRobotViper650::setPosition(const vpRobot::vpControlFrameType frame, - const vpColVector &position) +void vpRobotViper650::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) { if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) { @@ -1225,17 +1192,15 @@ void vpRobotViper650::setPosition(const vpRobot::vpControlFrameType frame, // std::cout << "Base frame destination Rzyz (deg): " << destination.t() // << std::endl; - Try(PrimitiveMOVE_C_Viper650(destination.data, configuration, - positioningVelocity)); + 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."); + throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: " + "Mixt frame not implemented."); } } @@ -1256,8 +1221,7 @@ void vpRobotViper650::setPosition(const vpRobot::vpControlFrameType frame, if (TryStt < 0 || error < 0) { vpERROR_TRACE("Positionning error."); - throw vpRobotException(vpRobotException::positionOutOfRangeError, - "Position out of range."); + throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range."); } return; @@ -1321,17 +1285,15 @@ int main() robot.setPositioningVelocity(20); // Moves the robot in the camera frame - robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, -pos6); #endif + 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) +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); @@ -1397,8 +1359,7 @@ void vpRobotViper650::setPosition(const std::string &filename) if (ret == false) { vpERROR_TRACE("Bad position in \"%s\"", filename.c_str()); - throw vpRobotException(vpRobotException::lowLevelError, - "Bad position in filename."); + throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename."); } this->setRobotState(vpRobot::STATE_POSITION_CONTROL); this->setPosition(vpRobot::ARTICULAR_FRAME, q); @@ -1456,8 +1417,7 @@ int main() } // Create a rotation matrix from the Rxyz rotation angles - vpRotationMatrix fRc(frc); // reference frame to camera frame rotation -matrix + vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix // Create the camera to fix frame transformation in terms of a // homogeneous matrix @@ -1473,8 +1433,7 @@ matrix vpColVector & r) */ -void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &position, double ×tamp) +void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double ×tamp) { InitTry; @@ -1518,17 +1477,15 @@ void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, } 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"); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: " + "not implemented"); } } CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get position."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get position."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position."); } return; @@ -1544,8 +1501,7 @@ void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, The difference is here that the timestamp is not used. */ -void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &position) +void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) { double timestamp; getPosition(frame, position, timestamp); @@ -1564,8 +1520,7 @@ void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &) */ -void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, - vpPoseVector &position, double ×tamp) +void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp) { vpColVector posRxyz; // recupere position en Rxyz @@ -1593,8 +1548,7 @@ void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, The difference is here that the timestamp is not returned. */ -void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, - vpPoseVector &position) +void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position) { double timestamp; getPosition(frame, position, timestamp); @@ -1689,17 +1643,14 @@ int main() } \endcode */ -void vpRobotViper650::setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &vel) +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) "); + 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); @@ -1726,8 +1677,7 @@ void vpRobotViper650::setVelocity(const vpRobot::vpControlFrameType frame, vpColVector vel_max(6); // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) { - if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < - std::numeric_limits::epsilon()) { + if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits::epsilon()) { for (unsigned int i = 0; i < 6; i++) vel_max[i] = getMaxRotationVelocity(); } else { @@ -1778,12 +1728,10 @@ void vpRobotViper650::setVelocity(const vpRobot::vpControlFrameType frame, if (TryStt < 0) { if (TryStt == VelStopOnJoint) { UInt32 axisInJoint[njoint]; - PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, - NULL); + 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; + std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl; } } else { printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt); @@ -1860,8 +1808,7 @@ int main() } \endcode */ -void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &velocity, double ×tamp) +void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double ×tamp) { velocity.resize(6); velocity = 0; @@ -1952,8 +1899,7 @@ void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get velocity."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get velocity."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity."); } } @@ -1965,8 +1911,7 @@ void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &, double &) except that the timestamp is not returned. */ -void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &velocity) +void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity) { double timestamp; getVelocity(frame, velocity, timestamp); @@ -2022,8 +1967,7 @@ int main() } \endcode */ -vpColVector vpRobotViper650::getVelocity(vpRobot::vpControlFrameType frame, - double ×tamp) +vpColVector vpRobotViper650::getVelocity(vpRobot::vpControlFrameType frame, double ×tamp) { vpColVector velocity; getVelocity(frame, velocity, timestamp); @@ -2081,8 +2025,11 @@ R: 0.1 0.3 -0.25 -80.5 80 0 an error occurs. The code below shows how to read a position from a file and move the robot to -this position. \code #include #include - #include +this position. +\code +#include +#include +#include int main() { @@ -2103,8 +2050,8 @@ int main() 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 + robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position +#endif } \endcode @@ -2130,10 +2077,8 @@ bool vpRobotViper650::readPosFile(const std::string &filename, vpColVector &q) while (std::getline(fd, line)) { lineNum++; if (lineNum == 1) { - if (!(line.compare(0, id.size(), id) == - 0)) { // check if Viper650 position file - std::cout << "Error: this position file " << filename - << " is not for Viper650 robot" << std::endl; + if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper650 position file + std::cout << "Error: this position file " << filename << " is not for Viper650 robot" << std::endl; return false; } } @@ -2142,8 +2087,7 @@ bool vpRobotViper650::readPosFile(const std::string &filename, vpColVector &q) } if ((line.compare(0, key.size(), key) == 0)) { // decode position // check if there are at least njoint values in the line - std::vector chain = - vpIoTools::splitChain(line, std::string(" ")); + std::vector chain = vpIoTools::splitChain(line, std::string(" ")); if (chain.size() < njoint + 1) // try to split with tab separator chain = vpIoTools::splitChain(line, std::string("\t")); if (chain.size() < njoint + 1) @@ -2165,8 +2109,7 @@ bool vpRobotViper650::readPosFile(const std::string &filename, vpColVector &q) fd.close(); if (!pos_found) { - std::cout << "Error: unable to find a position for Viper650 robot in " - << filename << std::endl; + std::cout << "Error: unable to find a position for Viper650 robot in " << filename << std::endl; return false; } @@ -2196,8 +2139,7 @@ bool vpRobotViper650::readPosFile(const std::string &filename, vpColVector &q) \sa readPosFile() */ -bool vpRobotViper650::savePosFile(const std::string &filename, - const vpColVector &q) +bool vpRobotViper650::savePosFile(const std::string &filename, const vpColVector &q) { FILE *fd; @@ -2214,9 +2156,8 @@ bool vpRobotViper650::savePosFile(const std::string &filename, #\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])); + 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); @@ -2264,8 +2205,7 @@ void vpRobotViper650::move(const std::string &filename) Euler Rxyz representation. */ -void vpRobotViper650::getDisplacement(vpRobot::vpControlFrameType frame, - vpColVector &displacement) +void vpRobotViper650::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement) { displacement.resize(6); displacement = 0; @@ -2314,8 +2254,7 @@ void vpRobotViper650::getDisplacement(vpRobot::vpControlFrameType frame, CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get velocity."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get velocity."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity."); } } @@ -2344,8 +2283,7 @@ void vpRobotViper650::biasForceTorqueSensor() const CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot bias the force/torque sensor."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot bias the force/torque sensor."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor."); } } @@ -2400,8 +2338,7 @@ void vpRobotViper650::getForceTorque(vpColVector &H) const CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get the force/torque measures."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get force/torque measures."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures."); } } @@ -2429,9 +2366,9 @@ int main() robot.biasForceTorqueSensor(); for (unsigned int i=0; i< 10; i++) { - vpColVector H = robot.getForceTorque(); // Get force/torque measures [Fx, -Fy, Fz, Tx, Ty, Tz] std::cout << "Measured force/torque: " << H.t() << -std::endl; vpTime::wait(5); + vpColVector H = robot.getForceTorque(); // Get force/torque measures [Fx, Fy, Fz, Tx, Ty, Tz] + std::cout << "Measured force/torque: " << H.t() << std::endl; + vpTime::wait(5); } #endif } @@ -2456,8 +2393,7 @@ vpColVector vpRobotViper650::getForceTorque() const CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get the force/torque measures."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get force/torque measures."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures."); } return H; // Here to avoid a warning, but should never be called } @@ -2475,8 +2411,7 @@ void vpRobotViper650::enableJoint6Limits() const CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot enable joint limits on axis 6"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot enable joint limits on axis 6."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6."); } } @@ -2498,8 +2433,7 @@ void vpRobotViper650::disableJoint6Limits() const CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot disable joint limits on axis 6"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot disable joint limits on axis 6."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6."); } } @@ -2535,7 +2469,6 @@ void vpRobotViper650::setMaxRotationVelocity(double w_max) robot.setVelocity(ARTICULAR_FRAME, v); \endcode - \param w6_max : Maximum rotation velocity expressed in rad/s on joint 6. */ @@ -2552,10 +2485,7 @@ void vpRobotViper650::setMaxRotationVelocityJoint6(const double w6_max) \return Maximum rotation velocity on joint 6 expressed in rad/s. */ -double vpRobotViper650::getMaxRotationVelocityJoint6() const -{ - return maxRotationVelocity_joint6; -} +double vpRobotViper650::getMaxRotationVelocityJoint6() const { return maxRotationVelocity_joint6; } /*! @@ -2570,8 +2500,7 @@ void vpRobotViper650::openGripper() std::cout << "Open the pneumatic gripper..." << std::endl; CatchPrint(); if (TryStt < 0) { - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot open the gripper."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper."); } } @@ -2589,8 +2518,7 @@ void vpRobotViper650::closeGripper() const std::cout << "Close the pneumatic gripper..." << std::endl; CatchPrint(); if (TryStt < 0) { - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot close the gripper."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper."); } } diff --git a/modules/robot/src/real-robot/viper/vpRobotViper850.cpp b/modules/robot/src/real-robot/viper/vpRobotViper850.cpp index 7e3ae776ce..fc3abb11bc 100644 --- a/modules/robot/src/real-robot/viper/vpRobotViper850.cpp +++ b/modules/robot/src/real-robot/viper/vpRobotViper850.cpp @@ -82,8 +82,7 @@ const double vpRobotViper850::defaultPositioningVelocity = 15.0; */ void emergencyStopViper850(int signo) { - std::cout << "Stop the Viper850 application by signal (" << signo - << "): " << (char)7; + std::cout << "Stop the Viper850 application by signal (" << signo << "): " << (char)7; switch (signo) { case SIGINT: std::cout << "SIGINT (stop by ^C) " << std::endl; @@ -149,8 +148,7 @@ int main() // Set the extrinsic camera parameters obtained with a perpective // projection model including a distorsion parameter - robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA, - vpCameraParameters::perspectiveProjWithDistortion); + robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA, vpCameraParameters::perspectiveProjWithDistortion); \endcode Now, you can get the intrinsic camera parameters associated to an @@ -269,12 +267,9 @@ void vpRobotViper850::init(void) #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI) std::string calibfile; #ifdef VISP_HAVE_VIPER850_DATA - calibfile = - std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal"); + calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal"); if (!vpIoTools::checkFilename(calibfile)) - throw(vpException(vpException::ioError, - "ATI F/T calib file \"%s\" doesn't exist", - calibfile.c_str())); + throw(vpException(vpException::ioError, "ATI F/T calib file \"%s\" doesn't exist", calibfile.c_str())); #else throw(vpException(vpException::ioError, "You don't have access to Viper850 " "data to retrive ATI F/T calib " @@ -301,8 +296,7 @@ void vpRobotViper850::init(void) // Look if the power is on or off UInt32 HIPowerStatus; UInt32 EStopStatus; - Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); // Print the robot status @@ -329,8 +323,7 @@ void vpRobotViper850::init(void) 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 << "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 } @@ -366,8 +359,7 @@ void vpRobotViper850::init(void) ShutDownConnection(); std::cout << "Cannot open connection with the motionblox..." << std::endl; - throw vpRobotException(vpRobotException::constructionError, - "Cannot open connection with the motionblox"); + throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox"); } return; } @@ -403,8 +395,7 @@ int main() // Set the extrinsic camera parameters obtained with a perpective // projection model including a distorsion parameter - robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA, - vpCameraParameters::perspectiveProjWithDistortion); + robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA, vpCameraParameters::perspectiveProjWithDistortion); \endcode Now, you can get the intrinsic camera parameters associated to an @@ -430,9 +421,7 @@ int main() init(vpViper850::vpToolType, const vpHomogeneousMatrix&), init(vpViper850::vpToolType, const std::string&) */ -void vpRobotViper850::init( - vpViper850::vpToolType tool, - vpCameraParameters::vpCameraParametersProjType projModel) +void vpRobotViper850::init(vpViper850::vpToolType tool, vpCameraParameters::vpCameraParametersProjType projModel) { // Read the robot constants from files // - joint [min,max], coupl_56, long_56 @@ -515,8 +504,7 @@ eMc_TRANS_XYZ 0.05 0.01 0.06 vpCameraParameters::vpCameraParametersProjType), init(vpViper850::vpToolType, const vpHomogeneousMatrix&) */ -void vpRobotViper850::init(vpViper850::vpToolType tool, - const std::string &filename) +void vpRobotViper850::init(vpViper850::vpToolType tool, const std::string &filename) { vpViper850::init(tool, filename); @@ -582,8 +570,7 @@ int main() vpCameraParameters::vpCameraParametersProjType), init(vpViper850::vpToolType, const std::string&) */ -void vpRobotViper850::init(vpViper850::vpToolType tool, - const vpHomogeneousMatrix &eMc_) +void vpRobotViper850::init(vpViper850::vpToolType tool, const vpHomogeneousMatrix &eMc_) { vpViper850::init(tool, eMc_); @@ -656,8 +643,7 @@ void vpRobotViper850::set_eMc(const vpHomogeneousMatrix &eMc_) \param erc_ : Rotation between the end-effector frame and the tool frame using the Euler angles in radians with the XYZ convention. */ -void vpRobotViper850::set_eMc(const vpTranslationVector &etc_, - const vpRxyzVector &erc_) +void vpRobotViper850::set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_) { this->vpViper850::set_eMc(etc_, erc_); @@ -699,8 +685,7 @@ vpRobotViper850::~vpRobotViper850(void) // Look if the power is on or off UInt32 HIPowerStatus; - Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); // if (HIPowerStatus == 1) { @@ -725,8 +710,7 @@ Change the robot state. \param newState : New requested robot state. */ -vpRobot::vpRobotStateType -vpRobotViper850::setRobotState(vpRobot::vpRobotStateType newState) +vpRobot::vpRobotStateType vpRobotViper850::setRobotState(vpRobot::vpRobotStateType newState) { InitTry; @@ -735,15 +719,13 @@ vpRobotViper850::setRobotState(vpRobot::vpRobotStateType newState) // Start primitive STOP only if the current state is Velocity if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) { Try(PrimitiveSTOP_Viper850()); - vpTime::sleepMs( - 100); // needed to ensure velocity task ends up on low level + vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level } break; } case vpRobot::STATE_POSITION_CONTROL: { if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) { - std::cout - << "Change the control mode from velocity to position control.\n"; + 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 @@ -791,8 +773,7 @@ void vpRobotViper850::stopMotion(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot stop robot motion"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot stop robot motion."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion."); } } @@ -816,8 +797,7 @@ void vpRobotViper850::powerOn(void) unsigned int nitermax = 10; for (unsigned int i = 0; i < nitermax; i++) { - Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus)); if (EStopStatus == ESTOP_AUTO) { controlMode = AUTO; break; // exit for loop @@ -837,8 +817,7 @@ void vpRobotViper850::powerOn(void) fflush(stdout); CAL_Wait(1); } else { - std::cout << "Sorry there is an error on the emergency chain." - << std::endl; + 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(); @@ -851,8 +830,7 @@ void vpRobotViper850::powerOn(void) if (EStopStatus == ESTOP_ACTIVATED) { std::cout << "Sorry, cannot power on the robot." << std::endl; - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot power on the robot."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."); } if (HIPowerStatus == 0) { @@ -865,8 +843,7 @@ void vpRobotViper850::powerOn(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot power on the robot"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot power off the robot."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."); } } @@ -885,8 +862,7 @@ void vpRobotViper850::powerOff(void) // Look if the power is on or off UInt32 HIPowerStatus; - Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); if (HIPowerStatus == 1) { @@ -899,8 +875,7 @@ void vpRobotViper850::powerOff(void) CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot power off the robot"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot power off the robot."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."); } } @@ -921,8 +896,7 @@ bool vpRobotViper850::getPowerState(void) const bool status = false; // Look if the power is on or off UInt32 HIPowerStatus; - Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, - &HIPowerStatus)); + Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus)); CAL_Wait(0.1); if (HIPowerStatus == 1) { @@ -932,8 +906,7 @@ bool vpRobotViper850::getPowerState(void) const CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get the power status"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get the power status."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status."); } return status; } @@ -966,10 +939,7 @@ void vpRobotViper850::get_cVe(vpVelocityTwistMatrix &cVe) const end-effector frame. */ -void vpRobotViper850::get_cMe(vpHomogeneousMatrix &cMe) const -{ - vpViper850::get_cMe(cMe); -} +void vpRobotViper850::get_cMe(vpHomogeneousMatrix &cMe) const { vpViper850::get_cMe(cMe); } /*! @@ -1074,20 +1044,14 @@ int main() \sa getPositioningVelocity() */ -void vpRobotViper850::setPositioningVelocity(const double velocity) -{ - positioningVelocity = velocity; -} +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; -} +double vpRobotViper850::getPositioningVelocity(void) const { return positioningVelocity; } /*! @@ -1167,8 +1131,7 @@ int main() \endcode */ -void vpRobotViper850::setPosition(const vpRobot::vpControlFrameType frame, - const vpColVector &position) +void vpRobotViper850::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) { if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) { @@ -1249,17 +1212,15 @@ void vpRobotViper850::setPosition(const vpRobot::vpControlFrameType frame, // std::cout << "Base frame destination Rzyz (deg): " << destination.t() // << std::endl; - Try(PrimitiveMOVE_C_Viper850(destination.data, configuration, - positioningVelocity)); + Try(PrimitiveMOVE_C_Viper850(destination.data, configuration, positioningVelocity)); Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000)); break; } case vpRobot::MIXT_FRAME: { vpERROR_TRACE("Positionning error. Mixt frame not implemented"); - throw vpRobotException(vpRobotException::lowLevelError, - "Positionning error: " - "Mixt frame not implemented."); + throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: " + "Mixt frame not implemented."); } } @@ -1279,8 +1240,7 @@ void vpRobotViper850::setPosition(const vpRobot::vpControlFrameType frame, if (TryStt < 0 || error < 0) { vpERROR_TRACE("Positionning error."); - throw vpRobotException(vpRobotException::positionOutOfRangeError, - "Position out of range."); + throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range."); } return; @@ -1344,17 +1304,15 @@ int main() robot.setPositioningVelocity(20); // Moves the robot in the camera frame - robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, -pos6); #endif + 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) +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); @@ -1420,8 +1378,7 @@ void vpRobotViper850::setPosition(const std::string &filename) if (ret == false) { vpERROR_TRACE("Bad position in \"%s\"", filename.c_str()); - throw vpRobotException(vpRobotException::lowLevelError, - "Bad position in filename."); + throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename."); } this->setRobotState(vpRobot::STATE_POSITION_CONTROL); this->setPosition(vpRobot::ARTICULAR_FRAME, q); @@ -1479,8 +1436,7 @@ int main() } // Create a rotation matrix from the Rxyz rotation angles - vpRotationMatrix fRc(frc); // reference frame to camera frame rotation -matrix + vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix // Create the camera to fix frame transformation in terms of a // homogeneous matrix @@ -1496,8 +1452,7 @@ matrix vpColVector & r) */ -void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &position, double ×tamp) +void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double ×tamp) { InitTry; @@ -1541,17 +1496,15 @@ void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, } 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"); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: " + "not implemented"); } } CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get position."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get position."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position."); } return; @@ -1578,8 +1531,7 @@ double vpRobotViper850::getTime() const The difference is here that the timestamp is not used. */ -void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &position) +void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) { double timestamp; getPosition(frame, position, timestamp); @@ -1596,8 +1548,7 @@ void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, u}\f$ representation. */ -void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, - vpPoseVector &position, double ×tamp) +void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp) { vpColVector posRxyz; // recupere position en Rxyz @@ -1625,8 +1576,7 @@ void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, The difference is here that the timestamp is not used. */ -void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, - vpPoseVector &position) +void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position) { vpColVector posRxyz; double timestamp; @@ -1723,17 +1673,14 @@ int main() } \endcode */ -void vpRobotViper850::setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &vel) +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) "); + 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); @@ -1760,8 +1707,7 @@ void vpRobotViper850::setVelocity(const vpRobot::vpControlFrameType frame, vpColVector vel_max(6); // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) { - if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < - std::numeric_limits::epsilon()) { + if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits::epsilon()) { for (unsigned int i = 0; i < 6; i++) vel_max[i] = getMaxRotationVelocity(); @@ -1813,12 +1759,10 @@ void vpRobotViper850::setVelocity(const vpRobot::vpControlFrameType frame, if (TryStt < 0) { if (TryStt == VelStopOnJoint) { UInt32 axisInJoint[njoint]; - PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, - NULL); + 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; + std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl; } } else { printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt); @@ -1895,8 +1839,7 @@ int main() } \endcode */ -void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &velocity, double ×tamp) +void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double ×tamp) { velocity.resize(6); @@ -1988,8 +1931,7 @@ void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get velocity."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get velocity."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity."); } } @@ -2001,8 +1943,7 @@ void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &, double &) except that the timestamp is not returned. */ -void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &velocity) +void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity) { double timestamp; getVelocity(frame, velocity, timestamp); @@ -2058,8 +1999,7 @@ int main() } \endcode */ -vpColVector vpRobotViper850::getVelocity(vpRobot::vpControlFrameType frame, - double ×tamp) +vpColVector vpRobotViper850::getVelocity(vpRobot::vpControlFrameType frame, double ×tamp) { vpColVector velocity; getVelocity(frame, velocity, timestamp); @@ -2117,8 +2057,11 @@ R: 0.1 0.3 -0.25 -80.5 80 0 an error occurs. The code below shows how to read a position from a file and move the robot to -this position. \code #include #include - #include +this position. +\code +#include +#include +#include int main() { @@ -2139,8 +2082,8 @@ int main() 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 + robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position +#endif } \endcode @@ -2166,10 +2109,8 @@ bool vpRobotViper850::readPosFile(const std::string &filename, vpColVector &q) while (std::getline(fd, line)) { lineNum++; if (lineNum == 1) { - if (!(line.compare(0, id.size(), id) == - 0)) { // check if Viper850 position file - std::cout << "Error: this position file " << filename - << " is not for Viper850 robot" << std::endl; + if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file + std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl; return false; } } @@ -2178,8 +2119,7 @@ bool vpRobotViper850::readPosFile(const std::string &filename, vpColVector &q) } if ((line.compare(0, key.size(), key) == 0)) { // decode position // check if there are at least njoint values in the line - std::vector chain = - vpIoTools::splitChain(line, std::string(" ")); + std::vector chain = vpIoTools::splitChain(line, std::string(" ")); if (chain.size() < njoint + 1) // try to split with tab separator chain = vpIoTools::splitChain(line, std::string("\t")); if (chain.size() < njoint + 1) @@ -2201,8 +2141,7 @@ bool vpRobotViper850::readPosFile(const std::string &filename, vpColVector &q) fd.close(); if (!pos_found) { - std::cout << "Error: unable to find a position for Viper850 robot in " - << filename << std::endl; + std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl; return false; } @@ -2232,8 +2171,7 @@ bool vpRobotViper850::readPosFile(const std::string &filename, vpColVector &q) \sa readPosFile() */ -bool vpRobotViper850::savePosFile(const std::string &filename, - const vpColVector &q) +bool vpRobotViper850::savePosFile(const std::string &filename, const vpColVector &q) { FILE *fd; @@ -2250,9 +2188,8 @@ bool vpRobotViper850::savePosFile(const std::string &filename, #\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])); + 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); @@ -2300,8 +2237,7 @@ void vpRobotViper850::move(const std::string &filename) Euler Rxyz representation. */ -void vpRobotViper850::getDisplacement(vpRobot::vpControlFrameType frame, - vpColVector &displacement) +void vpRobotViper850::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement) { displacement.resize(6); displacement = 0; @@ -2350,8 +2286,7 @@ void vpRobotViper850::getDisplacement(vpRobot::vpControlFrameType frame, CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get velocity."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get velocity."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity."); } } @@ -2368,9 +2303,8 @@ void vpRobotViper850::biasForceTorqueSensor() #if defined(VISP_HAVE_COMEDI) ati.bias(); #else - throw(vpException(vpException::fatalError, - "Cannot use ATI F/T if comedi is not installed. Try sudo " - "apt-get install libcomedi-dev")); + throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo " + "apt-get install libcomedi-dev")); #endif #else // Use serial link InitTry; @@ -2383,8 +2317,7 @@ void vpRobotViper850::biasForceTorqueSensor() CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot bias the force/torque sensor."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot bias the force/torque sensor."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor."); } #endif } @@ -2402,9 +2335,8 @@ void vpRobotViper850::unbiasForceTorqueSensor() #if defined(VISP_HAVE_COMEDI) ati.unbias(); #else - throw(vpException(vpException::fatalError, - "Cannot use ATI F/T if comedi is not installed. Try sudo " - "apt-get install libcomedi-dev")); + throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo " + "apt-get install libcomedi-dev")); #endif #else // Use serial link // Not implemented @@ -2458,9 +2390,8 @@ void vpRobotViper850::getForceTorque(vpColVector &H) const H = ati.getForceTorque(); #else (void)H; - throw(vpException(vpException::fatalError, - "Cannot use ATI F/T if comedi is not installed. Try sudo " - "apt-get install libcomedi-dev")); + throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo " + "apt-get install libcomedi-dev")); #endif #else // Use serial link InitTry; @@ -2472,8 +2403,7 @@ void vpRobotViper850::getForceTorque(vpColVector &H) const CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get the force/torque measures."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get force/torque measures."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures."); } #endif } @@ -2502,8 +2432,8 @@ int main() robot.biasForceTorqueSensor(); for (unsigned int i=0; i< 10; i++) { - vpColVector H = robot.getForceTorque(); // force/torque measures [Fx, Fy, -Fz, Tx, Ty, Tz] std::cout << "Measured force/torque: " << H.t() << std::endl; + vpColVector H = robot.getForceTorque(); // force/torque measures [Fx, Fy, Fz, Tx, Ty, Tz] + std::cout << "Measured force/torque: " << H.t() << std::endl; vpTime::wait(5); } #endif @@ -2523,9 +2453,8 @@ vpColVector vpRobotViper850::getForceTorque() const vpColVector H = ati.getForceTorque(); return H; #else - throw(vpException(vpException::fatalError, - "Cannot use ATI F/T if comedi is not installed. Try sudo " - "apt-get install libcomedi-dev")); + throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo " + "apt-get install libcomedi-dev")); #endif #else // Use serial link InitTry; @@ -2538,8 +2467,7 @@ vpColVector vpRobotViper850::getForceTorque() const CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot get the force/torque measures."); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot get force/torque measures."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures."); } return H; // Here to avoid a warning, but should never be called #endif @@ -2558,8 +2486,7 @@ void vpRobotViper850::openGripper() std::cout << "Open the pneumatic gripper..." << std::endl; CatchPrint(); if (TryStt < 0) { - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot open the gripper."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper."); } } @@ -2577,8 +2504,7 @@ void vpRobotViper850::closeGripper() const std::cout << "Close the pneumatic gripper..." << std::endl; CatchPrint(); if (TryStt < 0) { - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot close the gripper."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper."); } } @@ -2595,8 +2521,7 @@ void vpRobotViper850::enableJoint6Limits() const CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot enable joint limits on axis 6"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot enable joint limits on axis 6."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6."); } } @@ -2618,8 +2543,7 @@ void vpRobotViper850::disableJoint6Limits() const CatchPrint(); if (TryStt < 0) { vpERROR_TRACE("Cannot disable joint limits on axis 6"); - throw vpRobotException(vpRobotException::lowLevelError, - "Cannot disable joint limits on axis 6."); + throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6."); } } @@ -2672,10 +2596,7 @@ void vpRobotViper850::setMaxRotationVelocityJoint6(const double w6_max) \return Maximum rotation velocity on joint 6 expressed in rad/s. */ -double vpRobotViper850::getMaxRotationVelocityJoint6() const -{ - return maxRotationVelocity_joint6; -} +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 diff --git a/modules/robot/src/real-robot/viper/vpViper.cpp b/modules/robot/src/real-robot/viper/vpViper.cpp index 5be413200c..29157c8b69 100644 --- a/modules/robot/src/real-robot/viper/vpViper.cpp +++ b/modules/robot/src/real-robot/viper/vpViper.cpp @@ -63,8 +63,7 @@ const unsigned int vpViper::njoint = 6; */ vpViper::vpViper() - : eMc(), etc(), erc(), a1(0), d1(0), a2(), a3(), d4(0), d6(0), d7(0), - c56(0), joint_max(), joint_min() + : eMc(), etc(), erc(), a1(0), d1(0), a2(), a3(), d4(0), d6(0), d7(0), c56(0), joint_max(), joint_min() { // Default values are initialized @@ -141,8 +140,7 @@ vpHomogeneousMatrix vpViper::getForwardKinematics(const vpColVector &q) const \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, +bool vpViper::convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod, const bool &verbose) const { double eps = 0.01; @@ -162,9 +160,8 @@ bool vpViper::convertJointPositionInLimits(unsigned int joint, } if (verbose) { - std::cout << "Joint " << joint - << " not in limits: " << this->joint_min[joint] << " < " << q - << " < " << this->joint_max[joint] << std::endl; + std::cout << "Joint " << joint << " not in limits: " << this->joint_min[joint] << " < " << q << " < " + << this->joint_max[joint] << std::endl; } return false; @@ -222,9 +219,8 @@ bool vpViper::convertJointPositionInLimits(unsigned int joint, \sa getForwardKinematics(), getInverseKinematics() */ -unsigned int -vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, - vpColVector &q, const bool &verbose) const +unsigned int vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, vpColVector &q, + const bool &verbose) const { vpColVector q_sol[8]; @@ -259,8 +255,7 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, // Compute q1 double a_2 = px * px + py * py; // if (a_2 == 0) {// singularity - if (std::fabs(a_2) <= - std::numeric_limits::epsilon()) { // singularity + if (std::fabs(a_2) <= std::numeric_limits::epsilon()) { // singularity c1[0] = cos(q[0]); s1[0] = sin(q[0]); c1[4] = cos(q[0] + M_PI); @@ -276,8 +271,7 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, 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) { + 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]; @@ -294,8 +288,7 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, 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 - + 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; @@ -308,8 +301,7 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, 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) { + 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); @@ -333,24 +325,17 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, 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)); + 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) { + 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]; @@ -385,8 +370,7 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, else q_sol[i][4] = M_PI; - if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod, verbose) == - true) { + 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; @@ -413,8 +397,7 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, #else q_sol[i][3] = atan2(s4s5, c4s5); #endif - if (convertJointPositionInLimits(3, q_sol[i][3], q4_mod, verbose) == - true) { + 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); @@ -425,8 +408,7 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, 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) { + 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); @@ -437,18 +419,13 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, // 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; + 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) { + if (convertJointPositionInLimits(4, q_sol[i + j][4], q5_mod, verbose) == true) { q_sol[i + j][4] = q5_mod; } else { @@ -467,20 +444,14 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, 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 + + 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 + + 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) { + if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod, verbose) == true) { q_sol[i][5] = q6_mod; } else { ok[i] = false; @@ -589,9 +560,7 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, \sa getForwardKinematics(), getInverseKinematicsWrist */ -unsigned int vpViper::getInverseKinematics(const vpHomogeneousMatrix &fMc, - vpColVector &q, - const bool &verbose) const +unsigned int vpViper::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose) const { vpHomogeneousMatrix fMw; vpHomogeneousMatrix wMe; @@ -731,8 +700,8 @@ int main() 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 + // 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 @@ -776,28 +745,20 @@ void vpViper::get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const 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[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[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[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; @@ -879,16 +840,12 @@ void vpViper::get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const 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[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[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; @@ -962,10 +919,7 @@ void vpViper::get_eMs(vpHomogeneousMatrix &eMs) const \sa get_eMc() */ -void vpViper::get_cMe(vpHomogeneousMatrix &cMe) const -{ - cMe = this->eMc.inverse(); -} +void vpViper::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->eMc.inverse(); } /*! @@ -1291,8 +1245,7 @@ void vpViper::set_eMc(const vpHomogeneousMatrix &eMc_) \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_) +void vpViper::set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_) { this->etc = etc_; this->erc = erc_; @@ -1331,13 +1284,11 @@ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpViper &viper) << "\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; + << "\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 index d595f5bb8b..46c5e3767c 100644 --- a/modules/robot/src/real-robot/viper/vpViper650.cpp +++ b/modules/robot/src/real-robot/viper/vpViper650.cpp @@ -49,71 +49,49 @@ #include #include -static const char *opt_viper650[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", - NULL}; +static const char *opt_viper650[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", NULL}; #ifdef VISP_HAVE_VIPER650_DATA -const std::string - vpViper650::CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + - std::string( - "/include/const_eMc_MarlinF033C_without_distortion_Viper650.cnf"); - -const std::string - vpViper650::CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + - std::string( - "/include/const_eMc_MarlinF033C_with_distortion_Viper650.cnf"); - -const std::string - vpViper650::CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + - std::string( - "/include/const_eMc_PTGreyFlea2_without_distortion_Viper650.cnf"); - -const std::string - vpViper650::CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + - std::string( - "/include/const_eMc_PTGreyFlea2_with_distortion_Viper650.cnf"); - -const std::string - vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + - std::string("/include/" - "const_eMc_schunk_gripper_without_distortion_Viper650." - "cnf"); - -const std::string - vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + - std::string( - "/include/const_eMc_schunk_gripper_with_distortion_Viper650.cnf"); +const std::string vpViper650::CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME = + std::string(VISP_VIPER650_DATA_PATH) + + std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper650.cnf"); -const std::string vpViper650::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME = +const std::string vpViper650::CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME = + std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper650.cnf"); + +const std::string vpViper650::CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME = std::string(VISP_VIPER650_DATA_PATH) + - std::string("/include/const_eMc_generic_without_distortion_Viper650.cnf"); + std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper650.cnf"); -const std::string vpViper650::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME = +const std::string vpViper650::CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME = + std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper650.cnf"); + +const std::string vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME = + std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/" + "const_eMc_schunk_gripper_without_distortion_Viper650." + "cnf"); + +const std::string vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME = std::string(VISP_VIPER650_DATA_PATH) + - std::string("/include/const_eMc_generic_with_distortion_Viper650.cnf"); + std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper650.cnf"); + +const std::string vpViper650::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME = + std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper650.cnf"); + +const std::string vpViper650::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME = + std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper650.cnf"); const std::string vpViper650::CONST_CAMERA_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + - std::string("/include/const_camera_Viper650.xml"); + std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_camera_Viper650.xml"); #endif // VISP_HAVE_VIPER650_DATA -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_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; +const vpViper650::vpToolType vpViper650::defaultTool = vpViper650::TOOL_PTGREY_FLEA2_CAMERA; /*! @@ -122,8 +100,7 @@ const vpViper650::vpToolType vpViper650::defaultTool = */ vpViper650::vpViper650() - : tool_current(vpViper650::defaultTool), - projModel(vpCameraParameters::perspectiveProjWithoutDistortion) + : tool_current(vpViper650::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion) { // Denavit Hartenberg parameters a1 = 0.075; @@ -131,8 +108,7 @@ vpViper650::vpViper650() 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 + 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 @@ -200,9 +176,7 @@ void vpViper650::init(const std::string &camera_extrinsic_parameters) init(vpViper650::vpToolType, const vpHomogeneousMatrix&) */ -void vpViper650::init( - vpViper650::vpToolType tool, - vpCameraParameters::vpCameraParametersProjType proj_model) +void vpViper650::init(vpViper650::vpToolType tool, vpCameraParameters::vpCameraParametersProjType proj_model) { this->projModel = proj_model; @@ -256,11 +230,10 @@ void vpViper650::init( break; } case vpViper650::TOOL_CUSTOM: { - throw vpRobotException( - vpRobotException::badValue, - "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" + "You should use init(vpViper650::vpToolType, const std::string&) or" + "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead"); } default: { vpERROR_TRACE("This error should not occur!"); @@ -341,11 +314,10 @@ void vpViper650::init( break; } case vpViper650::TOOL_CUSTOM: { - throw vpRobotException( - vpRobotException::badValue, - "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" + "You should use init(vpViper650::vpToolType, const std::string&) or" + "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead"); } } vpRotationMatrix eRc(erc); @@ -386,8 +358,7 @@ eMc_TRANS_XYZ 0.05 0.01 0.06 vpCameraParameters::vpCameraParametersProjType), init(vpViper650::vpToolType, const vpHomogeneousMatrix&) */ -void vpViper650::init(vpViper650::vpToolType tool, - const std::string &filename) +void vpViper650::init(vpViper650::vpToolType tool, const std::string &filename) { this->setToolType(tool); this->parseConfigFile(filename.c_str()); @@ -408,8 +379,7 @@ void vpViper650::init(vpViper650::vpToolType tool, init(vpViper650::vpToolType, const std::string&) */ -void vpViper650::init(vpViper650::vpToolType tool, - const vpHomogeneousMatrix &eMc_) +void vpViper650::init(vpViper650::vpToolType tool, const vpHomogeneousMatrix &eMc_) { this->setToolType(tool); this->set_eMc(eMc_); @@ -431,8 +401,7 @@ void vpViper650::parseConfigFile(const std::string &filename) std::ifstream fdconfig(filename.c_str(), std::ios::in); if (!fdconfig.is_open()) { - throw vpRobotException(vpRobotException::readingParametersError, - "Impossible to read the config file: %s", + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s", filename.c_str()); } @@ -444,8 +413,7 @@ void vpViper650::parseConfigFile(const std::string &filename) while (std::getline(fdconfig, line)) { lineNum++; - if ((line.compare(0, 1, "#") == 0) || - line.empty()) { // skip comment or empty line + if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line continue; } std::istringstream ss(line); @@ -478,8 +446,7 @@ void vpViper650::parseConfigFile(const std::string &filename) } default: - throw(vpRobotException(vpRobotException::readingParametersError, - "Bad configuration file %s line #%d", + throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d", filename.c_str(), lineNum)); } } @@ -558,8 +525,7 @@ int main() 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 << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl; } std::cout << "Camera parameters: " << cam << std::endl; } @@ -570,76 +536,53 @@ parameters are not found. */ -void vpViper650::getCameraParameters(vpCameraParameters &cam, - const unsigned int &image_width, +void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const { #if defined(VISP_HAVE_XML2) && defined(VISP_HAVE_VIPER650_DATA) vpXmlParserCamera parser; switch (getToolType()) { case vpViper650::TOOL_MARLIN_F033C_CAMERA: { - std::cout << "Get camera parameters for camera \"" - << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\"" + 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."); + << "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::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."); + << "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::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."); + << "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."); + 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: { - throw vpRobotException( - vpRobotException::badValue, - "No intrinsic parameters available for a custom tool"); + throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool"); } default: { vpERROR_TRACE("This error should not occur!"); @@ -651,8 +594,7 @@ void vpViper650::getCameraParameters(vpCameraParameters &cam, // "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."); + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); } } #else @@ -661,23 +603,20 @@ void vpViper650::getCameraParameters(vpCameraParameters &cam, 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::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); + 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."); + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); } break; } @@ -685,57 +624,49 @@ void vpViper650::getCameraParameters(vpCameraParameters &cam, 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::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); + 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."); + 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; + 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); + 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."); + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); } break; } case vpViper650::TOOL_CUSTOM: { - throw vpRobotException( - vpRobotException::badValue, - "No intrinsic parameters available for a custom tool"); + throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool"); } default: vpERROR_TRACE("This error should not occur!"); - throw vpRobotException(vpRobotException::readingParametersError, - "Impossible to read the camera parameters."); + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); break; } #endif @@ -795,8 +726,7 @@ int main() robot.getCameraParameters (cam, I); } catch(...) { - std::cout << "Cannot get camera parameters for image: " << I.getWidth() << -" x " << I.getHeight() << std::endl; + std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl; } std::cout << "Camera parameters: " << cam << std::endl; } @@ -806,8 +736,7 @@ int main() parameters are not found. */ -void vpViper650::getCameraParameters(vpCameraParameters &cam, - const vpImage &I) const +void vpViper650::getCameraParameters(vpCameraParameters &cam, const vpImage &I) const { getCameraParameters(cam, I.getWidth(), I.getHeight()); } @@ -864,8 +793,7 @@ int main() robot.getCameraParameters (cam, I); } catch(...) { - std::cout << "Cannot get camera parameters for image: " << I.getWidth() << -" x " << I.getHeight() << std::endl; + std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl; } std::cout << "Camera parameters: " << cam << std::endl; } @@ -876,8 +804,7 @@ parameters are not found. */ -void vpViper650::getCameraParameters(vpCameraParameters &cam, - const vpImage &I) const +void vpViper650::getCameraParameters(vpCameraParameters &cam, const vpImage &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 index 978b0224a4..8d0506d91b 100644 --- a/modules/robot/src/real-robot/viper/vpViper850.cpp +++ b/modules/robot/src/real-robot/viper/vpViper850.cpp @@ -49,71 +49,49 @@ #include #include -static const char *opt_viper850[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", - NULL}; +static const char *opt_viper850[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", NULL}; #ifdef VISP_HAVE_VIPER850_DATA -const std::string - vpViper850::CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + - std::string( - "/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf"); - -const std::string - vpViper850::CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + - std::string( - "/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf"); - -const std::string - vpViper850::CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + - std::string( - "/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf"); - -const std::string - vpViper850::CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + - std::string( - "/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf"); - -const std::string - vpViper850::CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + - std::string("/include/" - "const_eMc_schunk_gripper_without_distortion_Viper850." - "cnf"); - -const std::string - vpViper850::CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + - std::string( - "/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf"); +const std::string vpViper850::CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME = + std::string(VISP_VIPER850_DATA_PATH) + + std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf"); -const std::string vpViper850::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME = +const std::string vpViper850::CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME = + std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf"); + +const std::string vpViper850::CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME = std::string(VISP_VIPER850_DATA_PATH) + - std::string("/include/const_eMc_generic_without_distortion_Viper850.cnf"); + std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf"); -const std::string vpViper850::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME = +const std::string vpViper850::CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME = + std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf"); + +const std::string vpViper850::CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME = + std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/" + "const_eMc_schunk_gripper_without_distortion_Viper850." + "cnf"); + +const std::string vpViper850::CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME = std::string(VISP_VIPER850_DATA_PATH) + - std::string("/include/const_eMc_generic_with_distortion_Viper850.cnf"); + std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf"); + +const std::string vpViper850::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME = + std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper850.cnf"); + +const std::string vpViper850::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME = + std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper850.cnf"); const std::string vpViper850::CONST_CAMERA_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + - std::string("/include/const_camera_Viper850.xml"); + std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_camera_Viper850.xml"); #endif // VISP_HAVE_VIPER850_DATA -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_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; +const vpViper850::vpToolType vpViper850::defaultTool = vpViper850::TOOL_PTGREY_FLEA2_CAMERA; /*! @@ -122,8 +100,7 @@ const vpViper850::vpToolType vpViper850::defaultTool = */ vpViper850::vpViper850() - : tool_current(vpViper850::defaultTool), - projModel(vpCameraParameters::perspectiveProjWithoutDistortion) + : tool_current(vpViper850::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion) { // Denavit Hartenberg parameters @@ -132,8 +109,7 @@ vpViper850::vpViper850() 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 + 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 @@ -201,9 +177,7 @@ void vpViper850::init(const std::string &camera_extrinsic_parameters) init(vpViper850::vpToolType, const vpHomogeneousMatrix&) */ -void vpViper850::init( - vpViper850::vpToolType tool, - vpCameraParameters::vpCameraParametersProjType proj_model) +void vpViper850::init(vpViper850::vpToolType tool, vpCameraParameters::vpCameraParametersProjType proj_model) { this->projModel = proj_model; @@ -257,11 +231,10 @@ void vpViper850::init( break; } case vpViper850::TOOL_CUSTOM: { - throw vpRobotException( - vpRobotException::badValue, - "No predefined file available for a custom tool" - "You should use init(vpViper850::vpToolType, const std::string&) or" - "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead"); + throw vpRobotException(vpRobotException::badValue, + "No predefined file available for a custom tool" + "You should use init(vpViper850::vpToolType, const std::string&) or" + "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead"); } default: { vpERROR_TRACE("This error should not occur!"); @@ -342,11 +315,10 @@ void vpViper850::init( break; } case vpViper850::TOOL_CUSTOM: { - throw vpRobotException( - vpRobotException::badValue, - "No predefined parameters available for a custom tool" - "You should use init(vpViper850::vpToolType, const std::string&) or" - "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead"); + throw vpRobotException(vpRobotException::badValue, + "No predefined parameters available for a custom tool" + "You should use init(vpViper850::vpToolType, const std::string&) or" + "init(vpViper850::vpToolType, const vpHomogeneousMatrix&) instead"); } } vpRotationMatrix eRc(erc); @@ -387,8 +359,7 @@ eMc_TRANS_XYZ 0.05 0.01 0.06 vpCameraParameters::vpCameraParametersProjType), init(vpViper850::vpToolType, const vpHomogeneousMatrix&) */ -void vpViper850::init(vpViper850::vpToolType tool, - const std::string &filename) +void vpViper850::init(vpViper850::vpToolType tool, const std::string &filename) { this->setToolType(tool); this->parseConfigFile(filename.c_str()); @@ -409,8 +380,7 @@ void vpViper850::init(vpViper850::vpToolType tool, init(vpViper850::vpToolType, const std::string&) */ -void vpViper850::init(vpViper850::vpToolType tool, - const vpHomogeneousMatrix &eMc_) +void vpViper850::init(vpViper850::vpToolType tool, const vpHomogeneousMatrix &eMc_) { this->setToolType(tool); this->set_eMc(eMc_); @@ -432,8 +402,7 @@ void vpViper850::parseConfigFile(const std::string &filename) std::ifstream fdconfig(filename.c_str(), std::ios::in); if (!fdconfig.is_open()) { - throw vpRobotException(vpRobotException::readingParametersError, - "Impossible to read the config file: %s", + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s", filename.c_str()); } @@ -445,8 +414,7 @@ void vpViper850::parseConfigFile(const std::string &filename) while (std::getline(fdconfig, line)) { lineNum++; - if ((line.compare(0, 1, "#") == 0) || - line.empty()) { // skip comment or empty line + if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line continue; } std::istringstream ss(line); @@ -479,8 +447,7 @@ void vpViper850::parseConfigFile(const std::string &filename) } default: - throw(vpRobotException(vpRobotException::readingParametersError, - "Bad configuration file %s line #%d", + throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d", filename.c_str(), lineNum)); } } @@ -559,8 +526,7 @@ int main() 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 << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl; } std::cout << "Camera parameters: " << cam << std::endl; } @@ -571,76 +537,53 @@ parameters are not found. */ -void vpViper850::getCameraParameters(vpCameraParameters &cam, - const unsigned int &image_width, +void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const { #if defined(VISP_HAVE_XML2) && defined(VISP_HAVE_VIPER850_DATA) vpXmlParserCamera parser; switch (getToolType()) { case vpViper850::TOOL_MARLIN_F033C_CAMERA: { - std::cout << "Get camera parameters for camera \"" - << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" + 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."); + << "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::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."); + << "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::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."); + << "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."); + 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; } case vpViper850::TOOL_CUSTOM: { - throw vpRobotException( - vpRobotException::badValue, - "No intrinsic parameters available for a custom tool"); + throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool"); } default: { vpERROR_TRACE("This error should not occur!"); @@ -652,8 +595,7 @@ void vpViper850::getCameraParameters(vpCameraParameters &cam, // "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."); + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); } } #else @@ -662,23 +604,20 @@ void vpViper850::getCameraParameters(vpCameraParameters &cam, 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::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); + 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."); + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); } break; } @@ -686,57 +625,49 @@ void vpViper850::getCameraParameters(vpCameraParameters &cam, 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::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); + 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."); + 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; + 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); + 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."); + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); } break; } case vpViper850::TOOL_CUSTOM: { - throw vpRobotException( - vpRobotException::badValue, - "No intrinsic parameters available for a custom tool"); + throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool"); } default: vpERROR_TRACE("This error should not occur!"); - throw vpRobotException(vpRobotException::readingParametersError, - "Impossible to read the camera parameters."); + throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); } #endif return; @@ -795,8 +726,7 @@ int main() robot.getCameraParameters (cam, I); } catch(...) { - std::cout << "Cannot get camera parameters for image: " << I.getWidth() << -" x " << I.getHeight() << std::endl; + std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl; } std::cout << "Camera parameters: " << cam << std::endl; } @@ -806,8 +736,7 @@ int main() parameters are not found. */ -void vpViper850::getCameraParameters(vpCameraParameters &cam, - const vpImage &I) const +void vpViper850::getCameraParameters(vpCameraParameters &cam, const vpImage &I) const { getCameraParameters(cam, I.getWidth(), I.getHeight()); } @@ -864,8 +793,7 @@ int main() robot.getCameraParameters (cam, I); } catch(...) { - std::cout << "Cannot get camera parameters for image: " << I.getWidth() << -" x " << I.getHeight() << std::endl; + std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl; } std::cout << "Camera parameters: " << cam << std::endl; } @@ -876,8 +804,7 @@ parameters are not found. */ -void vpViper850::getCameraParameters(vpCameraParameters &cam, - const vpImage &I) const +void vpViper850::getCameraParameters(vpCameraParameters &cam, const vpImage &I) const { getCameraParameters(cam, I.getWidth(), I.getHeight()); } diff --git a/modules/robot/src/robot-simulator/vpRobotCamera.cpp b/modules/robot/src/robot-simulator/vpRobotCamera.cpp index f2c3b8bf37..49e61cd07b 100644 --- a/modules/robot/src/robot-simulator/vpRobotCamera.cpp +++ b/modules/robot/src/robot-simulator/vpRobotCamera.cpp @@ -88,9 +88,8 @@ void vpRobotCamera::init() 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 + 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 } /*! @@ -146,8 +145,7 @@ void vpRobotCamera::get_eJe(vpMatrix &eJe_) { eJe_ = this->eJe; } \sa setSamplingTime() */ -void vpRobotCamera::setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &v) +void vpRobotCamera::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v) { switch (frame) { case vpRobot::ARTICULAR_FRAME: @@ -165,23 +163,20 @@ void vpRobotCamera::setVelocity(const vpRobot::vpControlFrameType frame, vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true); - this->cMw_ = - vpExponentialMap::direct(v_sat, delta_t_).inverse() * this->cMw_; + 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"); + 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in the mixt frame:" + "functionality not implemented"); break; } @@ -191,10 +186,7 @@ void vpRobotCamera::setVelocity(const vpRobot::vpControlFrameType frame, Get the robot position as the transformation from camera frame to world frame. */ -void vpRobotCamera::getPosition(vpHomogeneousMatrix &cMw) const -{ - cMw = this->cMw_; -} +void vpRobotCamera::getPosition(vpHomogeneousMatrix &cMw) const { cMw = this->cMw_; } /* Get the current position of the robot. @@ -217,8 +209,7 @@ void vpRobotCamera::getPosition(vpHomogeneousMatrix &cMw) const 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) +void vpRobotCamera::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) { q.resize(6); @@ -244,9 +235,7 @@ void vpRobotCamera::getPosition(const vpRobot::vpControlFrameType frame, break; } case vpRobot::MIXT_FRAME: - std::cout - << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" - << std::endl; + std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl; } } diff --git a/modules/robot/src/robot-simulator/vpRobotWireFrameSimulator.cpp b/modules/robot/src/robot-simulator/vpRobotWireFrameSimulator.cpp index 2596378e81..c04de6dc94 100644 --- a/modules/robot/src/robot-simulator/vpRobotWireFrameSimulator.cpp +++ b/modules/robot/src/robot-simulator/vpRobotWireFrameSimulator.cpp @@ -38,8 +38,7 @@ #include -#if defined(VISP_HAVE_MODULE_GUI) && \ - ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) +#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) #include #include @@ -51,20 +50,19 @@ Basic constructor */ vpRobotWireFrameSimulator::vpRobotWireFrameSimulator() - : vpWireFrameSimulator(), vpRobotSimulator(), I(), tcur(0), tprev(0), - robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(), + : 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(), + 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) + displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false), setVelocityCalled(false), + verbose_(false) { setSamplingTime(0.010); velocity.resize(6); @@ -83,21 +81,20 @@ vpRobotWireFrameSimulator::vpRobotWireFrameSimulator() \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(), + : 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), + /* 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) + displayType(MODEL_3D), displayAllowed(do_display), constantSamplingTimeMode(false), setVelocityCalled(false), + verbose_(false) { setSamplingTime(0.010); velocity.resize(6); @@ -132,8 +129,7 @@ vpRobotWireFrameSimulator::~vpRobotWireFrameSimulator() {} 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) +void vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object) { if (displayCamera) { free_Bound_scene(&(this->camera)); @@ -156,8 +152,7 @@ void vpRobotWireFrameSimulator::initScene( \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) +void vpRobotWireFrameSimulator::initScene(const char *obj, const char *desired_object) { if (displayCamera) { free_Bound_scene(&(this->camera)); @@ -236,17 +231,13 @@ void vpRobotWireFrameSimulator::getInternalView(vpImage &I_) 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::epsilon()) && - (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * - std::numeric_limits::epsilon())) { + if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits::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())); + 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; @@ -313,17 +304,13 @@ void vpRobotWireFrameSimulator::getInternalView(vpImage &I_) 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::epsilon()) && - (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * - std::numeric_limits::epsilon())) { + if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits::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())); + 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; diff --git a/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp b/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp index fb4d839a98..cbf2e0301d 100644 --- a/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp +++ b/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp @@ -37,8 +37,7 @@ *****************************************************************************/ #include -#if defined(VISP_HAVE_MODULE_GUI) && \ - ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) +#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) #include // std::fabs #include // numeric_limits #include @@ -61,9 +60,8 @@ 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() + : vpRobotWireFrameSimulator(), vpAfma6(), q_prev_getdis(), first_time_getdis(true), + positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir() { init(); initDisplay(); @@ -115,9 +113,8 @@ vpSimulatorAfma6::vpSimulatorAfma6() */ vpSimulatorAfma6::vpSimulatorAfma6(bool do_display) - : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), - first_time_getdis(true), positioningVelocity(defaultPositioningVelocity), - zeroPos(), reposPos(), toolCustom(false), arm_dir() + : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true), + positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir() { init(); initDisplay(); @@ -212,8 +209,7 @@ 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 arm_dirs = vpIoTools::splitChain( - std::string(VISP_ROBOT_ARMS_DIR), std::string(";")); + std::vector 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 @@ -224,11 +220,9 @@ void vpSimulatorAfma6::init() 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; + 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; + std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl; } } @@ -285,11 +279,9 @@ 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); + 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); @@ -313,20 +305,15 @@ void vpSimulatorAfma6::initDisplay() \sa vpCameraParameters, init() */ -void vpSimulatorAfma6::init( - vpAfma6::vpAfma6ToolType tool, - vpCameraParameters::vpCameraParametersProjType proj_model) +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" + 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"); + 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"); + throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator"); // Use here default values of the robot constant parameters. switch (tool) { @@ -338,8 +325,7 @@ void vpSimulatorAfma6::init( _etc[1] = 0.0033; // ty _etc[2] = 0.2272; // tz - setCameraParameters( - vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240)); + setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240)); if (robotArms != NULL) { while (get_displayBusy()) @@ -362,8 +348,7 @@ void vpSimulatorAfma6::init( _etc[1] = 0.1234; // ty _etc[2] = 0.1638; // tz - setCameraParameters( - vpCameraParameters(852.6583228197, 854.8084224761, 320, 240)); + setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240)); if (robotArms != NULL) { while (get_displayBusy()) @@ -386,8 +371,7 @@ void vpSimulatorAfma6::init( _etc[1] = 0.1281; // ty _etc[2] = 0.1658; // tz - setCameraParameters( - vpCameraParameters(853.4876600807, 856.0339170706, 320, 240)); + setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240)); if (robotArms != NULL) { while (get_displayBusy()) @@ -406,8 +390,7 @@ void vpSimulatorAfma6::init( } case vpAfma6::TOOL_CUSTOM: case vpAfma6::TOOL_GENERIC_CAMERA: { - std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" - << std::endl; + std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl; } } @@ -428,23 +411,20 @@ void vpSimulatorAfma6::init( \warning The image size must be : 640x480 ! */ -void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, - const unsigned int &image_width, +void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) { if (toolCustom) { - cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, - image_height / 2); + cam.initPersProjWithoutDistortion(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); + 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"); @@ -454,10 +434,9 @@ void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, 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); + 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"); @@ -467,8 +446,7 @@ void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, case vpAfma6::TOOL_CUSTOM: case vpAfma6::TOOL_GENERIC_CAMERA: case vpAfma6::TOOL_VACUUM: { - std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" - << std::endl; + std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl; break; } default: @@ -486,8 +464,7 @@ void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, \warning The image size must be : 640x480 ! */ -void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, - const vpImage &I_) +void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage &I_) { getCameraParameters(cam, I_.getWidth(), I_.getHeight()); } @@ -500,8 +477,7 @@ void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, \warning The image size must be : 640x480 ! */ -void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, - const vpImage &I_) +void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage &I_) { getCameraParameters(cam, I_.getWidth(), I_.getHeight()); } @@ -538,9 +514,9 @@ void vpSimulatorAfma6::updateArticularPosition() computeArticularVelocity(); double ellapsedTime = (tcur - tprev) * 1e-3; - if (constantSamplingTimeMode) { // if we want a constant velocity, we - // force the ellapsed time to the given - // samplingTime + if (constantSamplingTimeMode) { // if we want a constant velocity, we + // force the ellapsed time to the given + // samplingTime ellapsedTime = getSamplingTime(); // in second } @@ -548,16 +524,12 @@ void vpSimulatorAfma6::updateArticularPosition() 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]) { + 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; + 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; @@ -565,34 +537,25 @@ void vpSimulatorAfma6::updateArticularPosition() 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]; + 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)]) / + 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)]) / + 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]; + articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i]; jointLimit = true; jointLimitArt = (unsigned int)fabs((double)jl); @@ -605,10 +568,8 @@ void vpSimulatorAfma6::updateArticularPosition() 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_); + 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) { @@ -629,25 +590,20 @@ void vpSimulatorAfma6::updateArticularPosition() vpPoint pt(0, 0, 0); pt.track(getExternalCameraPosition()); - vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), - pt.get_y(), iP_1); + 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); + 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); + 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); + 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, + vpDisplay::displayCamera(I, getExternalCameraPosition() * fMit[7], cameraParam, 0.1, vpColor::green, thickness_); } @@ -815,8 +771,7 @@ Change the robot state. \param newState : New requested robot state. */ -vpRobot::vpRobotStateType -vpSimulatorAfma6::setRobotState(vpRobot::vpRobotStateType newState) +vpRobot::vpRobotStateType vpSimulatorAfma6::setRobotState(vpRobot::vpRobotStateType newState) { switch (newState) { case vpRobot::STATE_STOP: { @@ -828,8 +783,7 @@ vpSimulatorAfma6::setRobotState(vpRobot::vpRobotStateType newState) } case vpRobot::STATE_POSITION_CONTROL: { if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) { - std::cout - << "Change the control mode from velocity to position control.\n"; + std::cout << "Change the control mode from velocity to position control.\n"; stopMotion(); } else { // std::cout << "Change the control mode from stop to position @@ -925,17 +879,14 @@ int main() } \endcode */ -void vpSimulatorAfma6::setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &vel) +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) "); + 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); @@ -1143,8 +1094,7 @@ int main() } \endcode */ -void vpSimulatorAfma6::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &vel) +void vpSimulatorAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &vel) { vel.resize(6); @@ -1196,8 +1146,7 @@ void vpSimulatorAfma6::getVelocity(const vpRobot::vpControlFrameType frame, \sa getVelocity(const vpRobot::vpControlFrameType frame, vpColVector & vel) */ -void vpSimulatorAfma6::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &vel, double ×tamp) +void vpSimulatorAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &vel, double ×tamp) { timestamp = vpTime::measureTimeSecond(); getVelocity(frame, vel); @@ -1265,8 +1214,7 @@ vpColVector vpSimulatorAfma6::getVelocity(vpRobot::vpControlFrameType frame) \sa getVelocity(vpRobot::vpControlFrameType frame) */ -vpColVector vpSimulatorAfma6::getVelocity(vpRobot::vpControlFrameType frame, - double ×tamp) +vpColVector vpSimulatorAfma6::getVelocity(vpRobot::vpControlFrameType frame, double ×tamp) { timestamp = vpTime::measureTimeSecond(); vpColVector vel(6); @@ -1363,8 +1311,7 @@ int main() \endcode */ -void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, - const vpColVector &q) +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" @@ -1415,8 +1362,7 @@ void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, } } else { vpERROR_TRACE("Positionning error."); - throw vpRobotException(vpRobotException::positionOutOfRangeError, - "Position out of range."); + throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range."); } } while (errsqr > 1e-8 && nbSol > 0); @@ -1480,9 +1426,8 @@ void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, } case vpRobot::MIXT_FRAME: { vpERROR_TRACE("Positionning error. Mixt frame not implemented"); - throw vpRobotException(vpRobotException::lowLevelError, - "Positionning error: " - "Mixt frame not implemented."); + throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: " + "Mixt frame not implemented."); } } } @@ -1544,17 +1489,14 @@ int main() robot.setPositioningVelocity(20); // Moves the robot in the camera frame - robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, -pos6); + 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) +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); @@ -1616,8 +1558,7 @@ void vpSimulatorAfma6::setPosition(const char *filename) if (ret == false) { vpERROR_TRACE("Bad position in \"%s\"", filename); - throw vpRobotException(vpRobotException::lowLevelError, - "Bad position in filename."); + throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename."); } this->setRobotState(vpRobot::STATE_POSITION_CONTROL); this->setPosition(vpRobot::ARTICULAR_FRAME, q); @@ -1670,8 +1611,7 @@ int main() } // Create a rotation matrix from the Rxyz rotation angles - vpRotationMatrix fRc(frc); // reference frame to camera frame rotation -matrix + vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix // Create the camera to fix frame transformation in terms of a // homogeneous matrix @@ -1684,8 +1624,7 @@ double ×tamp) \sa setPosition(const vpRobot::vpControlFrameType frame, const vpColVector & r) */ -void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &q) +void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) { q.resize(6); @@ -1720,9 +1659,8 @@ void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, case vpRobot::MIXT_FRAME: { vpERROR_TRACE("Positionning error. Mixt frame not implemented"); - throw vpRobotException(vpRobotException::lowLevelError, - "Positionning error: " - "Mixt frame not implemented."); + throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: " + "Mixt frame not implemented."); } } } @@ -1753,8 +1691,7 @@ void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) */ -void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &q, double ×tamp) +void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp) { timestamp = vpTime::measureTimeSecond(); getPosition(frame, q); @@ -1771,8 +1708,7 @@ void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &) */ -void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, - vpPoseVector &position) +void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position) { vpColVector posRxyz; // recupere position en Rxyz @@ -1799,8 +1735,7 @@ void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, representation. */ -void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, - vpPoseVector &position, double ×tamp) +void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp) { timestamp = vpTime::measureTimeSecond(); getPosition(frame, position); @@ -1816,8 +1751,7 @@ void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, \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) +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 !"); @@ -1897,8 +1831,8 @@ int vpSimulatorAfma6::isInJointLimit() } if (artNumb != 0) - std::cout << "\nWarning: Velocity control stopped: axis " - << fabs((float)artNumb) << " on joint limit!" << std::endl; + std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" + << std::endl; return artNumb; } @@ -1920,8 +1854,7 @@ int vpSimulatorAfma6::isInJointLimit() Euler Rxyz representation. */ -void vpSimulatorAfma6::getDisplacement(vpRobot::vpControlFrameType frame, - vpColVector &displacement) +void vpSimulatorAfma6::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement) { displacement.resize(6); displacement = 0; @@ -1993,9 +1926,12 @@ expressed in meters, while joint rotations A,B,C in radians. 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); +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 @@ -2003,8 +1939,7 @@ robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position \sa savePosFile() */ -bool vpSimulatorAfma6::readPosFile(const std::string &filename, - vpColVector &q) +bool vpSimulatorAfma6::readPosFile(const std::string &filename, vpColVector &q) { std::ifstream fd(filename.c_str(), std::ios::in); @@ -2023,10 +1958,8 @@ bool vpSimulatorAfma6::readPosFile(const std::string &filename, while (std::getline(fd, line)) { lineNum++; if (lineNum == 1) { - if (!(line.compare(0, id.size(), id) == - 0)) { // check if Afma6 position file - std::cout << "Error: this position file " << filename - << " is not for Afma6 robot" << std::endl; + if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file + std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl; return false; } } @@ -2035,8 +1968,7 @@ bool vpSimulatorAfma6::readPosFile(const std::string &filename, } if ((line.compare(0, key.size(), key) == 0)) { // decode position // check if there are at least njoint values in the line - std::vector chain = - vpIoTools::splitChain(line, std::string(" ")); + std::vector chain = vpIoTools::splitChain(line, std::string(" ")); if (chain.size() < njoint + 1) // try to split with tab separator chain = vpIoTools::splitChain(line, std::string("\t")); if (chain.size() < njoint + 1) @@ -2060,8 +1992,7 @@ bool vpSimulatorAfma6::readPosFile(const std::string &filename, fd.close(); if (!pos_found) { - std::cout << "Error: unable to find a position for Afma6 robot in " - << filename << std::endl; + std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl; return false; } @@ -2090,8 +2021,7 @@ bool vpSimulatorAfma6::readPosFile(const std::string &filename, \sa readPosFile() */ -bool vpSimulatorAfma6::savePosFile(const std::string &filename, - const vpColVector &q) +bool vpSimulatorAfma6::savePosFile(const std::string &filename, const vpColVector &q) { FILE *fd; fd = fopen(filename.c_str(), "w"); @@ -2108,8 +2038,8 @@ bool vpSimulatorAfma6::savePosFile(const std::string &filename, #\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])); + 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); @@ -2144,10 +2074,7 @@ void vpSimulatorAfma6::move(const char *filename) \param cMe : Transformation between the camera frame and the end-effector frame. */ -void vpSimulatorAfma6::get_cMe(vpHomogeneousMatrix &cMe) -{ - vpAfma6::get_cMe(cMe); -} +void vpSimulatorAfma6::get_cMe(vpHomogeneousMatrix &cMe) { vpAfma6::get_cMe(cMe); } /*! Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to @@ -2237,12 +2164,10 @@ 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 scene_dirs = - vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";")); + std::vector 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 + if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists scene_dir_ = scene_dirs[i]; sceneDirExists = true; break; @@ -2250,23 +2175,18 @@ void vpSimulatorAfma6::initArms() if (!sceneDirExists) { try { scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR"); - std::cout << "The simulator uses data from VISP_SCENES_DIR=" - << scene_dir_ << std::endl; + 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; + 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" + 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"); + 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"); + throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator"); char *name_cam = new char[full_length]; @@ -2275,12 +2195,10 @@ void vpSimulatorAfma6::initArms() set_scene(name_cam, &camera, cameraFactor); if (arm_dir.size() > FILENAME_MAX) - throw vpException(vpException::dimensionError, - "Cannot initialize Afma6 simulator"); + 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"); + throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator"); char *name_arm = new char[full_length]; strcpy(name_arm, arm_dir.c_str()); @@ -2317,13 +2235,11 @@ void vpSimulatorAfma6::initArms() break; } case vpAfma6::TOOL_CUSTOM: { - std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" - << std::endl; + std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl; break; } case vpAfma6::TOOL_GENERIC_CAMERA: { - std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" - << std::endl; + std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl; break; } } @@ -2360,17 +2276,13 @@ void vpSimulatorAfma6::getExternalImage(vpImage &I_) 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::epsilon()) && - (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * - std::numeric_limits::epsilon())) { + if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits::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())); + 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; @@ -2439,8 +2351,7 @@ void vpSimulatorAfma6::getExternalImage(vpImage &I_) \return false if the robot kinematics is not able to reach the cMo position. */ -bool vpSimulatorAfma6::initialiseCameraRelativeToObject( - const vpHomogeneousMatrix &cMo_) +bool vpSimulatorAfma6::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_) { vpColVector stop(6); bool status = true; @@ -2451,8 +2362,7 @@ bool vpSimulatorAfma6::initialiseCameraRelativeToObject( fMc_ = fMo * cMo_.inverse(); vpColVector articularCoordinates = get_artCoord(); - int nbSol = - getInverseKinematics(fMc_, articularCoordinates, true, verbose_); + int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_); if (nbSol == 0) { status = false; @@ -2460,8 +2370,7 @@ bool vpSimulatorAfma6::initialiseCameraRelativeToObject( } if (verbose_) - std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() - << std::endl; + std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl; set_artCoord(articularCoordinates); @@ -2483,8 +2392,7 @@ bool vpSimulatorAfma6::initialiseCameraRelativeToObject( \param cMo_ : the desired pose of the camera. */ -void vpSimulatorAfma6::initialiseObjectRelativeToCamera( - const vpHomogeneousMatrix &cMo_) +void vpSimulatorAfma6::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_) { vpColVector stop(6); stop = 0; @@ -2506,9 +2414,7 @@ void vpSimulatorAfma6::initialiseObjectRelativeToCamera( \return True is the pose is reached, False else */ -bool vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, - vpImage *Iint, - const double &errMax) +bool vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage *Iint, const double &errMax) { // get rid of max velocity double vMax = getMaxTranslationVelocity(); diff --git a/modules/robot/src/robot-simulator/vpSimulatorCamera.cpp b/modules/robot/src/robot-simulator/vpSimulatorCamera.cpp index a2d5d2dfed..04fa744eea 100644 --- a/modules/robot/src/robot-simulator/vpSimulatorCamera.cpp +++ b/modules/robot/src/robot-simulator/vpSimulatorCamera.cpp @@ -71,9 +71,8 @@ void vpSimulatorCamera::init() 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 + 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 } /*! @@ -112,18 +111,12 @@ 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_; -} +void vpSimulatorCamera::getPosition(vpHomogeneousMatrix &wMc) const { wMc = this->wMc_; } /*! Return the camera position in the world frame. */ -vpHomogeneousMatrix vpSimulatorCamera::getPosition() const -{ - return (this->wMc_); -} +vpHomogeneousMatrix vpSimulatorCamera::getPosition() const { return (this->wMc_); } /* Get the current position of the camera. @@ -146,8 +139,7 @@ vpHomogeneousMatrix vpSimulatorCamera::getPosition() const 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) +void vpSimulatorCamera::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) { q.resize(6); @@ -173,9 +165,7 @@ void vpSimulatorCamera::getPosition(const vpRobot::vpControlFrameType frame, break; } case vpRobot::MIXT_FRAME: - std::cout - << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" - << std::endl; + std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl; } } @@ -201,8 +191,7 @@ void vpSimulatorCamera::getPosition(const vpRobot::vpControlFrameType frame, \sa setSamplingTime() */ -void vpSimulatorCamera::setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &v) +void vpSimulatorCamera::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v) { if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) { setRobotState(vpRobot::STATE_VELOCITY_CONTROL); @@ -227,16 +216,14 @@ void vpSimulatorCamera::setVelocity(const vpRobot::vpControlFrameType frame, 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"); + 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in the mixt frame:" + "functionality not implemented"); break; } diff --git a/modules/robot/src/robot-simulator/vpSimulatorPioneer.cpp b/modules/robot/src/robot-simulator/vpSimulatorPioneer.cpp index 442dfad5cd..e0c0f78c18 100644 --- a/modules/robot/src/robot-simulator/vpSimulatorPioneer.cpp +++ b/modules/robot/src/robot-simulator/vpSimulatorPioneer.cpp @@ -53,11 +53,7 @@ Constructor. */ -vpSimulatorPioneer::vpSimulatorPioneer() - : wMc_(), wMe_(), xm_(0), ym_(0), theta_(0) -{ - init(); -} +vpSimulatorPioneer::vpSimulatorPioneer() : wMc_(), wMe_(), xm_(0), ym_(0), theta_(0) { init(); } /*! Robot initialisation. @@ -95,10 +91,7 @@ vpSimulatorPioneer::~vpSimulatorPioneer() {} \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(); -} +void vpSimulatorPioneer::get_eJe(vpMatrix &_eJe) { _eJe = vpUnicycle::get_eJe(); } /*! Send to the controller a velocity. @@ -117,8 +110,7 @@ void vpSimulatorPioneer::get_eJe(vpMatrix &_eJe) \sa setSamplingTime() */ -void vpSimulatorPioneer::setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &v) +void vpSimulatorPioneer::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v) { switch (frame) { case vpRobot::ARTICULAR_FRAME: { @@ -130,8 +122,7 @@ void vpSimulatorPioneer::setVelocity(const vpRobot::vpControlFrameType 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"); + throw vpRobotException(vpRobotException::dimensionError, "Bad dimension of the control vector"); } vpColVector v_max(2); @@ -155,22 +146,19 @@ void vpSimulatorPioneer::setVelocity(const vpRobot::vpControlFrameType frame, 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"); + 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"); + 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"); + throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in the mixt frame:" + "functionality not implemented"); break; } @@ -180,10 +168,7 @@ void vpSimulatorPioneer::setVelocity(const vpRobot::vpControlFrameType frame, Get the robot position in the world frame. */ -void vpSimulatorPioneer::getPosition(vpHomogeneousMatrix &wMc) const -{ - wMc = this->wMc_; -} +void vpSimulatorPioneer::getPosition(vpHomogeneousMatrix &wMc) const { wMc = this->wMc_; } /* Get the current position of the robot. @@ -204,8 +189,7 @@ void vpSimulatorPioneer::getPosition(vpHomogeneousMatrix &wMc) const 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) +void vpSimulatorPioneer::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) { q.resize(6); @@ -235,8 +219,6 @@ void vpSimulatorPioneer::getPosition(const vpRobot::vpControlFrameType frame, break; } case vpRobot::MIXT_FRAME: - std::cout - << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" - << std::endl; + 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 index 354fcfb1ed..6e8b2a6892 100644 --- a/modules/robot/src/robot-simulator/vpSimulatorPioneerPan.cpp +++ b/modules/robot/src/robot-simulator/vpSimulatorPioneerPan.cpp @@ -57,11 +57,7 @@ setSamplingTime(). */ -vpSimulatorPioneerPan::vpSimulatorPioneerPan() - : wMc_(), wMm_(), xm_(0), ym_(0), theta_(0), q_pan_() -{ - init(); -} +vpSimulatorPioneerPan::vpSimulatorPioneerPan() : wMc_(), wMm_(), xm_(0), ym_(0), theta_(0), q_pan_() { init(); } /*! Robot initialisation. @@ -103,10 +99,7 @@ vpSimulatorPioneerPan::~vpSimulatorPioneerPan() {} \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(); -} +void vpSimulatorPioneerPan::get_eJe(vpMatrix &_eJe) { _eJe = vpUnicycle::get_eJe(); } /*! Send to the controller a velocity. @@ -126,8 +119,7 @@ void vpSimulatorPioneerPan::get_eJe(vpMatrix &_eJe) \sa setSamplingTime() */ -void vpSimulatorPioneerPan::setVelocity( - const vpRobot::vpControlFrameType frame, const vpColVector &v) +void vpSimulatorPioneerPan::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v) { switch (frame) { case vpRobot::ARTICULAR_FRAME: { @@ -140,8 +132,7 @@ void vpSimulatorPioneerPan::setVelocity( // 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"); + throw vpRobotException(vpRobotException::dimensionError, "Bad dimension of the control vector"); } vpColVector v_max(3); @@ -173,17 +164,14 @@ void vpSimulatorPioneerPan::setVelocity( break; } case vpRobot::CAMERA_FRAME: - throw vpRobotException(vpRobotException::wrongStateError, - "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"); case vpRobot::REFERENCE_FRAME: - throw vpRobotException(vpRobotException::wrongStateError, - "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"); case vpRobot::MIXT_FRAME: - throw vpRobotException(vpRobotException::wrongStateError, - "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"); } } @@ -191,10 +179,7 @@ void vpSimulatorPioneerPan::setVelocity( Get the robot position in the world frame. */ -void vpSimulatorPioneerPan::getPosition(vpHomogeneousMatrix &wMc) const -{ - wMc = this->wMc_; -} +void vpSimulatorPioneerPan::getPosition(vpHomogeneousMatrix &wMc) const { wMc = this->wMc_; } /* Get the current position of the robot. @@ -215,8 +200,7 @@ void vpSimulatorPioneerPan::getPosition(vpHomogeneousMatrix &wMc) const 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) +void vpSimulatorPioneerPan::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) { q.resize(6); @@ -246,8 +230,6 @@ void vpSimulatorPioneerPan::getPosition( break; } case vpRobot::MIXT_FRAME: - std::cout - << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" - << std::endl; + 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 index 6b0131e95d..1670db0601 100644 --- a/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp +++ b/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp @@ -38,8 +38,7 @@ #include -#if defined(VISP_HAVE_MODULE_GUI) && \ - ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) +#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD)) #include // std::fabs #include // numeric_limits @@ -61,9 +60,8 @@ 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() + : vpRobotWireFrameSimulator(), vpViper850(), q_prev_getdis(), first_time_getdis(true), + positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir() { init(); initDisplay(); @@ -115,9 +113,8 @@ vpSimulatorViper850::vpSimulatorViper850() */ vpSimulatorViper850::vpSimulatorViper850(bool do_display) - : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), - first_time_getdis(true), positioningVelocity(defaultPositioningVelocity), - zeroPos(), reposPos(), toolCustom(false), arm_dir() + : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true), + positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir() { init(); initDisplay(); @@ -213,8 +210,7 @@ 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 arm_dirs = vpIoTools::splitChain( - std::string(VISP_ROBOT_ARMS_DIR), std::string(";")); + std::vector 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 @@ -225,11 +221,9 @@ void vpSimulatorViper850::init() 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; + 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; + std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl; } } @@ -288,10 +282,8 @@ 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); + 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); @@ -315,9 +307,7 @@ void vpSimulatorViper850::initDisplay() \sa vpCameraParameters, init() */ -void vpSimulatorViper850::init( - vpViper850::vpToolType tool, - vpCameraParameters::vpCameraParametersProjType proj_model) +void vpSimulatorViper850::init(vpViper850::vpToolType tool, vpCameraParameters::vpCameraParametersProjType proj_model) { this->projModel = proj_model; @@ -348,8 +338,7 @@ void vpSimulatorViper850::init( case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: case vpViper850::TOOL_CUSTOM: case vpViper850::TOOL_GENERIC_CAMERA: { - std::cout << "This tool is not handled in vpSimulatorViper850.cpp" - << std::endl; + std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl; break; } } @@ -371,21 +360,18 @@ void vpSimulatorViper850::init( \warning The image size must be : 640x480 ! */ -void vpSimulatorViper850::getCameraParameters( - vpCameraParameters &cam, const unsigned int &image_width, - const unsigned int &image_height) +void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, + const unsigned int &image_height) { if (toolCustom) { - cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, - image_height / 2); + cam.initPersProjWithoutDistortion(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::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl; cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240); } else { @@ -397,8 +383,7 @@ void vpSimulatorViper850::getCameraParameters( 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::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl; cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240); } else { @@ -410,8 +395,7 @@ void vpSimulatorViper850::getCameraParameters( case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: case vpViper850::TOOL_CUSTOM: case vpViper850::TOOL_GENERIC_CAMERA: { - std::cout << "This tool is not handled in vpSimulatorViper850.cpp" - << std::endl; + std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl; break; } } @@ -426,8 +410,7 @@ void vpSimulatorViper850::getCameraParameters( \warning The image size must be : 640x480 ! */ -void vpSimulatorViper850::getCameraParameters( - vpCameraParameters &cam, const vpImage &I_) +void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, const vpImage &I_) { getCameraParameters(cam, I_.getWidth(), I_.getHeight()); } @@ -440,8 +423,7 @@ void vpSimulatorViper850::getCameraParameters( \warning The image size must be : 640x480 ! */ -void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, - const vpImage &I_) +void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, const vpImage &I_) { getCameraParameters(cam, I_.getWidth(), I_.getHeight()); } @@ -477,9 +459,9 @@ void vpSimulatorViper850::updateArticularPosition() computeArticularVelocity(); double ellapsedTime = (tcur - tprev) * 1e-3; - if (constantSamplingTimeMode) { // if we want a constant velocity, we - // force the ellapsed time to the given - // samplingTime + if (constantSamplingTimeMode) { // if we want a constant velocity, we + // force the ellapsed time to the given + // samplingTime ellapsedTime = getSamplingTime(); // in second } @@ -487,50 +469,37 @@ void vpSimulatorViper850::updateArticularPosition() 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]) { + 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; + 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]; + 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)]) / + 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)]) / + 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]; + articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i]; jointLimit = true; jointLimitArt = (unsigned int)fabs((double)jl); @@ -543,10 +512,8 @@ void vpSimulatorViper850::updateArticularPosition() 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_); + 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) { @@ -567,25 +534,20 @@ void vpSimulatorViper850::updateArticularPosition() vpPoint pt(0, 0, 0); pt.track(getExternalCameraPosition()); - vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), - pt.get_y(), iP_1); + 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); + 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); + 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); + 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, + vpDisplay::displayCamera(I, getExternalCameraPosition() * fMit[7], cameraParam, 0.1, vpColor::green, thickness_); } @@ -709,15 +671,11 @@ void vpSimulatorViper850::compute_fMi() 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][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][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; @@ -726,27 +684,18 @@ void vpSimulatorViper850::compute_fMi() 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][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][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; + 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); @@ -776,8 +725,7 @@ Change the robot state. \param newState : New requested robot state. */ -vpRobot::vpRobotStateType -vpSimulatorViper850::setRobotState(vpRobot::vpRobotStateType newState) +vpRobot::vpRobotStateType vpSimulatorViper850::setRobotState(vpRobot::vpRobotStateType newState) { switch (newState) { case vpRobot::STATE_STOP: { @@ -789,8 +737,7 @@ vpSimulatorViper850::setRobotState(vpRobot::vpRobotStateType newState) } case vpRobot::STATE_POSITION_CONTROL: { if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) { - std::cout - << "Change the control mode from velocity to position control.\n"; + std::cout << "Change the control mode from velocity to position control.\n"; stopMotion(); } else { // std::cout << "Change the control mode from stop to position @@ -886,17 +833,14 @@ int main() } \endcode */ -void vpSimulatorViper850::setVelocity(const vpRobot::vpControlFrameType frame, - const vpColVector &vel) +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) "); + 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); @@ -1105,8 +1049,7 @@ int main() } \endcode */ -void vpSimulatorViper850::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &vel) +void vpSimulatorViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &vel) { vel.resize(6); @@ -1158,8 +1101,7 @@ void vpSimulatorViper850::getVelocity(const vpRobot::vpControlFrameType frame, \sa getVelocity(const vpRobot::vpControlFrameType frame, vpColVector & vel) */ -void vpSimulatorViper850::getVelocity(const vpRobot::vpControlFrameType frame, - vpColVector &vel, double ×tamp) +void vpSimulatorViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &vel, double ×tamp) { timestamp = vpTime::measureTimeSecond(); getVelocity(frame, vel); @@ -1207,8 +1149,7 @@ int main() } \endcode */ -vpColVector -vpSimulatorViper850::getVelocity(vpRobot::vpControlFrameType frame) +vpColVector vpSimulatorViper850::getVelocity(vpRobot::vpControlFrameType frame) { vpColVector vel(6); getVelocity(frame, vel); @@ -1228,9 +1169,7 @@ vpSimulatorViper850::getVelocity(vpRobot::vpControlFrameType frame) \sa getVelocity(vpRobot::vpControlFrameType frame) */ -vpColVector -vpSimulatorViper850::getVelocity(vpRobot::vpControlFrameType frame, - double ×tamp) +vpColVector vpSimulatorViper850::getVelocity(vpRobot::vpControlFrameType frame, double ×tamp) { timestamp = vpTime::measureTimeSecond(); vpColVector vel(6); @@ -1327,8 +1266,7 @@ int main() \endcode */ -void vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame, - const vpColVector &q) +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" @@ -1379,8 +1317,7 @@ void vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame, } } else { vpERROR_TRACE("Positionning error."); - throw vpRobotException(vpRobotException::positionOutOfRangeError, - "Position out of range."); + throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range."); } } while (errsqr > 1e-8 && nbSol > 0); @@ -1444,9 +1381,8 @@ void vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame, } case vpRobot::MIXT_FRAME: { vpERROR_TRACE("Positionning error. Mixt frame not implemented"); - throw vpRobotException(vpRobotException::lowLevelError, - "Positionning error: " - "Mixt frame not implemented."); + throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: " + "Mixt frame not implemented."); } } } @@ -1508,17 +1444,14 @@ int main() robot.setPositioningVelocity(20); // Moves the robot in the camera frame - robot.setPosition(vpRobot::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, -pos6); + 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) +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); @@ -1580,8 +1513,7 @@ void vpSimulatorViper850::setPosition(const char *filename) if (ret == false) { vpERROR_TRACE("Bad position in \"%s\"", filename); - throw vpRobotException(vpRobotException::lowLevelError, - "Bad position in filename."); + throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename."); } this->setRobotState(vpRobot::STATE_POSITION_CONTROL); this->setPosition(vpRobot::ARTICULAR_FRAME, q); @@ -1634,8 +1566,7 @@ int main() } // Create a rotation matrix from the Rxyz rotation angles - vpRotationMatrix fRc(frc); // reference frame to camera frame rotation -matrix + vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix // Create the camera to fix frame transformation in terms of a // homogeneous matrix @@ -1648,8 +1579,7 @@ double ×tamp) \sa setPosition(const vpRobot::vpControlFrameType frame, const vpColVector & r) */ -void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &q) +void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) { q.resize(6); @@ -1684,9 +1614,8 @@ void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, case vpRobot::MIXT_FRAME: { vpERROR_TRACE("Positionning error. Mixt frame not implemented"); - throw vpRobotException(vpRobotException::lowLevelError, - "Positionning error: " - "Mixt frame not implemented."); + throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: " + "Mixt frame not implemented."); } } } @@ -1717,8 +1646,7 @@ void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) */ -void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, - vpColVector &q, double ×tamp) +void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp) { timestamp = vpTime::measureTimeSecond(); getPosition(frame, q); @@ -1735,8 +1663,7 @@ void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &) */ -void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, - vpPoseVector &position) +void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position) { vpColVector posRxyz; // recupere position en Rxyz @@ -1763,8 +1690,7 @@ void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, representation. */ -void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, - vpPoseVector &position, +void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp) { timestamp = vpTime::measureTimeSecond(); @@ -1779,8 +1705,7 @@ void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, 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) +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 !"); @@ -1905,8 +1830,8 @@ int vpSimulatorViper850::isInJointLimit() } if (artNumb != 0) - std::cout << "\nWarning: Velocity control stopped: axis " - << fabs((float)artNumb) << " on joint limit!" << std::endl; + std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" + << std::endl; return artNumb; } @@ -1928,8 +1853,7 @@ int vpSimulatorViper850::isInJointLimit() Euler Rxyz representation. */ -void vpSimulatorViper850::getDisplacement(vpRobot::vpControlFrameType frame, - vpColVector &displacement) +void vpSimulatorViper850::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement) { displacement.resize(6); displacement = 0; @@ -1999,8 +1923,11 @@ R: 0.1 0.3 -0.25 -80.5 80 0 an error occurs. The code below shows how to read a position from a file and move the robot to -this position. \code #include #include - #include +this position. +\code +#include +#include +#include int main() { @@ -2020,15 +1947,13 @@ int main() 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 + robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position } \endcode \sa savePosFile() */ -bool vpSimulatorViper850::readPosFile(const std::string &filename, - vpColVector &q) +bool vpSimulatorViper850::readPosFile(const std::string &filename, vpColVector &q) { std::ifstream fd(filename.c_str(), std::ios::in); @@ -2047,10 +1972,8 @@ bool vpSimulatorViper850::readPosFile(const std::string &filename, while (std::getline(fd, line)) { lineNum++; if (lineNum == 1) { - if (!(line.compare(0, id.size(), id) == - 0)) { // check if Viper850 position file - std::cout << "Error: this position file " << filename - << " is not for Viper850 robot" << std::endl; + if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file + std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl; return false; } } @@ -2059,8 +1982,7 @@ bool vpSimulatorViper850::readPosFile(const std::string &filename, } if ((line.compare(0, key.size(), key) == 0)) { // decode position // check if there are at least njoint values in the line - std::vector chain = - vpIoTools::splitChain(line, std::string(" ")); + std::vector chain = vpIoTools::splitChain(line, std::string(" ")); if (chain.size() < njoint + 1) // try to split with tab separator chain = vpIoTools::splitChain(line, std::string("\t")); if (chain.size() < njoint + 1) @@ -2082,8 +2004,7 @@ bool vpSimulatorViper850::readPosFile(const std::string &filename, fd.close(); if (!pos_found) { - std::cout << "Error: unable to find a position for Viper850 robot in " - << filename << std::endl; + std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl; return false; } @@ -2111,8 +2032,7 @@ bool vpSimulatorViper850::readPosFile(const std::string &filename, \sa readPosFile() */ -bool vpSimulatorViper850::savePosFile(const std::string &filename, - const vpColVector &q) +bool vpSimulatorViper850::savePosFile(const std::string &filename, const vpColVector &q) { FILE *fd; @@ -2129,9 +2049,8 @@ bool vpSimulatorViper850::savePosFile(const std::string &filename, #\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])); + 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); @@ -2166,10 +2085,7 @@ void vpSimulatorViper850::move(const char *filename) \param cMe : Transformation between the camera frame and the end-effector frame. */ -void vpSimulatorViper850::get_cMe(vpHomogeneousMatrix &cMe) -{ - vpViper850::get_cMe(cMe); -} +void vpSimulatorViper850::get_cMe(vpHomogeneousMatrix &cMe) { vpViper850::get_cMe(cMe); } /*! Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to @@ -2259,12 +2175,10 @@ 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 scene_dirs = - vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";")); + std::vector 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 + if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists scene_dir_ = scene_dirs[i]; sceneDirExists = true; break; @@ -2272,24 +2186,19 @@ void vpSimulatorViper850::initArms() if (!sceneDirExists) { try { scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR"); - std::cout << "The simulator uses data from VISP_SCENES_DIR=" - << scene_dir_ << std::endl; + 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; + 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" + 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"); + 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"); + throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator"); char *name_cam = new char[full_length]; @@ -2298,12 +2207,10 @@ void vpSimulatorViper850::initArms() set_scene(name_cam, &camera, cameraFactor); if (arm_dir.size() > FILENAME_MAX) - throw vpException(vpException::dimensionError, - "Cannot initialize Viper850 simulator"); + 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"); + throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator"); char *name_arm = new char[full_length]; strcpy(name_arm, arm_dir.c_str()); @@ -2362,17 +2269,13 @@ void vpSimulatorViper850::getExternalImage(vpImage &I_) 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::epsilon()) && - (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * - std::numeric_limits::epsilon())) { + if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits::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())); + 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; @@ -2440,8 +2343,7 @@ void vpSimulatorViper850::getExternalImage(vpImage &I_) \return false if the robot kinematics is not able to reach the cMo position. */ -bool vpSimulatorViper850::initialiseCameraRelativeToObject( - const vpHomogeneousMatrix &cMo_) +bool vpSimulatorViper850::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_) { vpColVector stop(6); bool status = true; @@ -2452,8 +2354,7 @@ bool vpSimulatorViper850::initialiseCameraRelativeToObject( fMc_ = fMo * cMo_.inverse(); vpColVector articularCoordinates = get_artCoord(); - unsigned int nbSol = - getInverseKinematics(fMc_, articularCoordinates, verbose_); + unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_); if (nbSol == 0) { status = false; @@ -2461,8 +2362,7 @@ bool vpSimulatorViper850::initialiseCameraRelativeToObject( } if (verbose_) - std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() - << std::endl; + std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl; set_artCoord(articularCoordinates); @@ -2484,8 +2384,7 @@ bool vpSimulatorViper850::initialiseCameraRelativeToObject( \param cMo_ : the desired pose of the camera. */ -void vpSimulatorViper850::initialiseObjectRelativeToCamera( - const vpHomogeneousMatrix &cMo_) +void vpSimulatorViper850::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_) { vpColVector stop(6); stop = 0; diff --git a/modules/robot/src/vpRobot.cpp b/modules/robot/src/vpRobot.cpp index 8002ce4ab7..2191d88193 100644 --- a/modules/robot/src/vpRobot.cpp +++ b/modules/robot/src/vpRobot.cpp @@ -52,19 +52,17 @@ const double vpRobot::maxRotationVelocityDefault = 0.7; 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) + 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) + maxTranslationVelocity(maxTranslationVelocityDefault), maxRotationVelocity(maxRotationVelocityDefault), nDof(0), + eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false), qmin(NULL), qmax(NULL), + verbose_(true) { *this = robot; } @@ -162,14 +160,11 @@ int main() } \endcode */ -vpColVector vpRobot::saturateVelocities(const vpColVector &v_in, - const vpColVector &v_max, - bool verbose) +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"); + 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++) { @@ -182,8 +177,7 @@ vpColVector vpRobot::saturateVelocities(const vpColVector &v_in, scale = scale_i; if (verbose) - std::cout << "Excess velocity " << v_in[i] << " axis nr. " << i - << std::endl; + std::cout << "Excess velocity " << v_in[i] << " axis nr. " << i << std::endl; } } @@ -204,15 +198,13 @@ vpColVector vpRobot::saturateVelocities(const vpColVector &v_in, \file vpRobot.cpp \brief class that defines a generic virtual robot */ -vpRobot::vpRobotStateType -vpRobot::setRobotState(const vpRobot::vpRobotStateType newState) +vpRobot::vpRobotStateType vpRobot::setRobotState(const vpRobot::vpRobotStateType newState) { stateRobot = newState; return newState; } -vpRobot::vpControlFrameType -vpRobot::setRobotFrame(vpRobot::vpControlFrameType newFrame) +vpRobot::vpControlFrameType vpRobot::setRobotFrame(vpRobot::vpControlFrameType newFrame) { frameRobot = newFrame; return newFrame; @@ -256,10 +248,7 @@ void vpRobot::setMaxTranslationVelocity(const double v_max) \return Maximum translation velocity expressed in m/s. */ -double vpRobot::getMaxTranslationVelocity(void) const -{ - return this->maxTranslationVelocity; -} +double vpRobot::getMaxTranslationVelocity(void) const { return this->maxTranslationVelocity; } /*! Set the maximal rotation velocity that can be sent to the robot during a @@ -281,7 +270,4 @@ void vpRobot::setMaxRotationVelocity(const double w_max) \return Maximum rotation velocity expressed in rad/s. */ -double vpRobot::getMaxRotationVelocity(void) const -{ - return this->maxRotationVelocity; -} +double vpRobot::getMaxRotationVelocity(void) const { return this->maxRotationVelocity; } diff --git a/modules/robot/src/vpRobotTemplate.cpp b/modules/robot/src/vpRobotTemplate.cpp index a3af83537b..4264f619a9 100644 --- a/modules/robot/src/vpRobotTemplate.cpp +++ b/modules/robot/src/vpRobotTemplate.cpp @@ -57,10 +57,7 @@ void vpRobotTemplate::init() vpRobotTemplate::vpRobotTemplate() { init(); } //! constructor -vpRobotTemplate::~vpRobotTemplate() -{ - std::cout << "Not implemented ! " << std::endl; -} +vpRobotTemplate::~vpRobotTemplate() { std::cout << "Not implemented ! " << std::endl; } /* @@ -72,16 +69,10 @@ get_fJe */ //! get the robot Jacobian expressed in the end-effector frame -void vpRobotTemplate::get_eJe(vpMatrix & /* _eJe */) -{ - std::cout << "Not implemented ! " << std::endl; -} +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; -} +void vpRobotTemplate::get_fJe(vpMatrix & /* _fJe */) { std::cout << "Not implemented ! " << std::endl; } /* @@ -109,9 +100,7 @@ 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 */) +void vpRobotTemplate::setVelocity(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* vel */) { std::cout << "Not implemented ! " << std::endl; } @@ -123,30 +112,21 @@ 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; -} +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; -} +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 */) +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 */) +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 */) +void vpRobotTemplate::getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) { std::cout << "Not implemented ! " << std::endl; } diff --git a/modules/robot/src/wireframe-simulator/vpArit.cpp b/modules/robot/src/wireframe-simulator/vpArit.cpp index b34e5fe8e9..6ea5095f74 100644 --- a/modules/robot/src/wireframe-simulator/vpArit.cpp +++ b/modules/robot/src/wireframe-simulator/vpArit.cpp @@ -56,8 +56,7 @@ void fprintf_matrix(FILE *fp, Matrix m) 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, "\t%.4f\t%.4f\t%.4f\t%.4f\n", m[i][0], m[i][1], m[i][2], m[i][3]); fprintf(fp, ")\n"); } @@ -96,8 +95,7 @@ void premult_matrix(Matrix a, Matrix b) 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]; + 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)); } @@ -198,8 +196,7 @@ void postmult_matrix(Matrix a, Matrix b) 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]; + 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)); } @@ -486,8 +483,7 @@ void rotate_vector(Vector *vp, float a, Vector *axis) 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); + SET_COORD3(*vp, u.x + v.x + cross.x, u.y + v.y + cross.y, u.z + v.z + cross.z); } /* @@ -571,15 +567,12 @@ void Matrix_to_Rotate(Matrix m, Vector *vp) 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)); + 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); + 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; @@ -625,11 +618,9 @@ void Position_to_Matrix(AritPosition *pp, Matrix m) 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); + 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); diff --git a/modules/robot/src/wireframe-simulator/vpArit.h b/modules/robot/src/wireframe-simulator/vpArit.h index 88931363d0..71c5df0275 100644 --- a/modules/robot/src/wireframe-simulator/vpArit.h +++ b/modules/robot/src/wireframe-simulator/vpArit.h @@ -44,110 +44,106 @@ #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_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 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_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 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 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_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 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 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 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 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 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_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 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_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 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 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 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)) @@ -173,12 +169,9 @@ typedef struct { float x, y, z; } Vector; -#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 \ - } \ +#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 } \ } /* @@ -198,22 +191,22 @@ typedef struct { Vector translate; /* vecteur translation */ } AritPosition; -#define IDENTITY_ROTATE \ - { \ - 0.0, 0.0, 0.0 \ +#define IDENTITY_ROTATE \ + { \ + 0.0, 0.0, 0.0 \ } -#define IDENTITY_SCALE \ - { \ - 1.0, 1.0, 1.0 \ +#define IDENTITY_SCALE \ + { \ + 1.0, 1.0, 1.0 \ } -#define IDENTITY_TRANSLATE \ - { \ - 0.0, 0.0, 0.0 \ +#define IDENTITY_TRANSLATE \ + { \ + 0.0, 0.0, 0.0 \ } -#define IDENTITY_POSITION \ - { \ - IDENTITY_ROTATE, IDENTITY_SCALE, IDENTITY_TRANSLATE \ +#define IDENTITY_POSITION \ + { \ + IDENTITY_ROTATE, IDENTITY_SCALE, IDENTITY_TRANSLATE \ } void fprintf_matrix(FILE *fp, Matrix m); diff --git a/modules/robot/src/wireframe-simulator/vpAritio.cpp b/modules/robot/src/wireframe-simulator/vpAritio.cpp index ce9b153696..67cd5ce803 100644 --- a/modules/robot/src/wireframe-simulator/vpAritio.cpp +++ b/modules/robot/src/wireframe-simulator/vpAritio.cpp @@ -57,9 +57,8 @@ */ 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); + 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); } /* @@ -69,8 +68,7 @@ void fprintf_Position(FILE *f, AritPosition *pp) */ void fscanf_Point3f(Point3f *pp) { - static const char *err_tbl[] = {"float expected (coordinate ", - " of point)"}; + static const char *err_tbl[] = {"float expected (coordinate ", " of point)"}; int t; /* Lecture de la premiere coordonnee du point. */ @@ -99,8 +97,7 @@ void fscanf_Point3f(Point3f *pp) */ void fscanf_Vector(Vector *vp) { - static const char *err_tbl[] = {"float expected (coordinate ", - " of vector)"}; + static const char *err_tbl[] = {"float expected (coordinate ", " of vector)"}; int t; diff --git a/modules/robot/src/wireframe-simulator/vpBound.cpp b/modules/robot/src/wireframe-simulator/vpBound.cpp index f9e7f8d199..2c03f981f8 100644 --- a/modules/robot/src/wireframe-simulator/vpBound.cpp +++ b/modules/robot/src/wireframe-simulator/vpBound.cpp @@ -139,8 +139,7 @@ void malloc_Bound(Bound *bp, Type type, int polygonal, Index fn, Index pn) if ((bp->point.nbr = pn) == 0) /* points */ bp->point.ptr = NULL; - else if ((bp->point.ptr = (Point3f *)malloc(pn * sizeof(Point3f))) == - NULL) { + else if ((bp->point.ptr = (Point3f *)malloc(pn * sizeof(Point3f))) == NULL) { perror(proc_name); exit(1); } @@ -184,8 +183,7 @@ void malloc_huge_Bound(Bound *bp) #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) { + if ((bp->face.ptr->vertex.ptr = (Index *)malloc(FACE_NBR * VERTEX_NBR * sizeof(Index))) == NULL) { static char proc_name[] = "malloc_Huge_Bound"; perror(proc_name); exit(1); @@ -205,8 +203,7 @@ 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) { + if ((bsp->name = (char *)malloc((strlen(name) + 1) * sizeof(char))) == NULL) { perror(proc_name); exit(1); } diff --git a/modules/robot/src/wireframe-simulator/vpBoundio.cpp b/modules/robot/src/wireframe-simulator/vpBoundio.cpp index dd52f7cc79..a92faefe48 100644 --- a/modules/robot/src/wireframe-simulator/vpBoundio.cpp +++ b/modules/robot/src/wireframe-simulator/vpBoundio.cpp @@ -157,8 +157,7 @@ void fscanf_Face_list(Face_list *lp) */ void fscanf_Point3f_list(Point3f_list *lp) { - static const char *err_tbl[] = {"float expected (coordinate ", - " of point)"}; + static const char *err_tbl[] = {"float expected (coordinate ", " of point)"}; Point3f *pp; /* point courant */ Point3f *pend; /* borne de "pp" */ diff --git a/modules/robot/src/wireframe-simulator/vpClipping.cpp b/modules/robot/src/wireframe-simulator/vpClipping.cpp index c5b38de8a5..ec2f6a6142 100644 --- a/modules/robot/src/wireframe-simulator/vpClipping.cpp +++ b/modules/robot/src/wireframe-simulator/vpClipping.cpp @@ -308,9 +308,7 @@ static void inter(Byte mask, Index v0, Index v1) /* t = (p0->w - p0->y) / ((p0->w - p0->y) - (p1->w - p1->y)); */ t = (p0->w - p0->y) - (p1->w - p1->y); // t = (t == 0) ? (float)1.0 : (p0->w - p0->y) / t; - t = (std::fabs(t) <= std::numeric_limits::epsilon()) - ? (float)1.0 - : (p0->w - p0->y) / t; + t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 : (p0->w - p0->y) / t; PAR_COORD3(*p, t, *p0, *p1); p->w = p->y; /* propriete du point d'intersection */ break; @@ -319,9 +317,7 @@ static void inter(Byte mask, Index v0, Index v1) /* t = (p0->w + p0->y) / ((p0->w + p0->y) - (p1->w + p1->y)); */ t = (p0->w + p0->y) - (p1->w + p1->y); // t = (t == 0) ? (float)1.0 : (p0->w + p0->y) / t; - t = (std::fabs(t) <= std::numeric_limits::epsilon()) - ? (float)1.0 - : (p0->w + p0->y) / t; + t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 : (p0->w + p0->y) / t; PAR_COORD3(*p, t, *p0, *p1); p->w = -p->y; /* propriete du point d'intersection */ break; @@ -330,9 +326,7 @@ static void inter(Byte mask, Index v0, Index v1) /* t = (p0->w - p0->x) / ((p0->w - p0->x) - (p1->w - p1->x)); */ t = (p0->w - p0->x) - (p1->w - p1->x); // t = (t == 0) ? (float)1.0 : (p0->w - p0->x) / t; - t = (std::fabs(t) <= std::numeric_limits::epsilon()) - ? (float)1.0 - : (p0->w - p0->x) / t; + t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 : (p0->w - p0->x) / t; PAR_COORD3(*p, t, *p0, *p1); p->w = p->x; /* propriete du point d'intersection */ break; @@ -341,9 +335,7 @@ static void inter(Byte mask, Index v0, Index v1) /* t = (p0->w + p0->x) / ((p0->w + p0->x) - (p1->w + p1->x)); */ t = (p0->w + p0->x) - (p1->w + p1->x); // t = (t == 0) ? (float)1.0 : (p0->w + p0->x) / t; - t = (std::fabs(t) <= std::numeric_limits::epsilon()) - ? (float)1.0 - : (p0->w + p0->x) / t; + t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 : (p0->w + p0->x) / t; PAR_COORD3(*p, t, *p0, *p1); p->w = -p->x; /* propriete du point d'intersection */ break; @@ -352,9 +344,7 @@ static void inter(Byte mask, Index v0, Index v1) /* t = (p0->w - p0->z) / ((p0->w - p0->z) - (p1->w - p1->z)); */ t = (p0->w - p0->z) - (p1->w - p1->z); // t = (t == 0) ? (float)1.0 : (p0->w - p0->z) / t; - t = (std::fabs(t) <= std::numeric_limits::epsilon()) - ? (float)1.0 - : (p0->w - p0->z) / t; + t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 : (p0->w - p0->z) / t; PAR_COORD3(*p, t, *p0, *p1); p->w = p->z; /* propriete du point d'intersection */ break; @@ -363,8 +353,7 @@ static void inter(Byte mask, Index v0, Index v1) /* t = p0->z / (p0->z - p1->z); */ t = (p0->z - p1->z); // t = (t == 0) ? (float)1.0 : p0->z / t; - t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 - : p0->z / t; + t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 : p0->z / t; p->x = (p1->x - p0->x) * t + p0->x; p->y = (p1->y - p0->y) * t + p0->y; p->w = (p1->w - p0->w) * t + p0->w; @@ -381,8 +370,7 @@ static void inter(Byte mask, Index v0, Index v1) 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); + 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 */ } diff --git a/modules/robot/src/wireframe-simulator/vpCoreDisplay.cpp b/modules/robot/src/wireframe-simulator/vpCoreDisplay.cpp index 81a92a26af..3a12423aaa 100644 --- a/modules/robot/src/wireframe-simulator/vpCoreDisplay.cpp +++ b/modules/robot/src/wireframe-simulator/vpCoreDisplay.cpp @@ -162,38 +162,32 @@ void set_Bound_face_display(Bound *bp, Byte b) 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)); + 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)); + 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)); + 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)); + 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)); + 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)); + fp->is_visible = ((p1->x - p0->x) * (p2->y - p0->y) <= (p1->y - p0->y) * (p2->x - p0->x)); } } } diff --git a/modules/robot/src/wireframe-simulator/vpLex.cpp b/modules/robot/src/wireframe-simulator/vpLex.cpp index db53ac4570..a91deb2510 100644 --- a/modules/robot/src/wireframe-simulator/vpLex.cpp +++ b/modules/robot/src/wireframe-simulator/vpLex.cpp @@ -782,8 +782,7 @@ void lexerr(const char *path, ...) count(); // write (STDERR, myline, mysptr - myline); - fprintf(stderr, "\n%*c\n\"%s\", line %d:\n", mycolumno, '^', source, - mylineno); + fprintf(stderr, "\n%*c\n\"%s\", line %d:\n", mycolumno, '^', source, mylineno); /* Affiche les messages d'erreur de la pile. */ diff --git a/modules/robot/src/wireframe-simulator/vpMy.h b/modules/robot/src/wireframe-simulator/vpMy.h index 05981beb77..6b971a6b88 100644 --- a/modules/robot/src/wireframe-simulator/vpMy.h +++ b/modules/robot/src/wireframe-simulator/vpMy.h @@ -76,18 +76,18 @@ //#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)) \ +#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); \ +#define SWAP(A, B, T) \ + { \ + (T) = (A); \ + (A) = (B); \ + (B) = (T); \ } typedef unsigned char Byte; diff --git a/modules/robot/src/wireframe-simulator/vpProjection.cpp b/modules/robot/src/wireframe-simulator/vpProjection.cpp index 067885a819..fe42bc2f96 100644 --- a/modules/robot/src/wireframe-simulator/vpProjection.cpp +++ b/modules/robot/src/wireframe-simulator/vpProjection.cpp @@ -146,8 +146,7 @@ void set_parallel(View_parameters *vp, Matrix wc) * 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); + 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); @@ -162,12 +161,10 @@ void set_parallel(View_parameters *vp, Matrix wc) * 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), + 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)), + 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); } @@ -219,11 +216,8 @@ void set_perspective(View_parameters *vp, Matrix wc) /* * 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))), + 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); /* diff --git a/modules/robot/src/wireframe-simulator/vpScene.cpp b/modules/robot/src/wireframe-simulator/vpScene.cpp index 220d9a7658..8e2c12b341 100644 --- a/modules/robot/src/wireframe-simulator/vpScene.cpp +++ b/modules/robot/src/wireframe-simulator/vpScene.cpp @@ -100,12 +100,9 @@ void set_scene(const char *str, Bound_scene *sc, float factor) if (std::fabs(factor) > std::numeric_limits::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; + 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; } } } @@ -127,10 +124,7 @@ void set_scene_wrl(const char *str, Bound_scene *sc, float factor) SoVRMLGroup *sceneGraphVRML2; if (!ok) { - throw(vpException( - vpException::fatalError, - "Can't open file \"%s\". Please check the Marker_Less.ini file", - str)); + throw(vpException(vpException::fatalError, "Can't open file \"%s\". Please check the Marker_Less.ini file", str)); } if (!in.isFileVRML2()) { @@ -148,8 +142,7 @@ void set_scene_wrl(const char *str, Bound_scene *sc, float factor) sceneGraphVRML2 = SoDB::readAllVRML(&in); if (sceneGraphVRML2 == NULL) { /*return -1;*/ - throw( - vpException(vpException::notInitialized, "Cannot read VRML file")); + throw(vpException(vpException::notInitialized, "Cannot read VRML file")); } sceneGraphVRML2->ref(); } @@ -170,8 +163,7 @@ void set_scene_wrl(const char *str, Bound_scene *sc, float factor) std::list ifs_list; SoChildList *child2list = child->getChildren(); for (int j = 0; j < child2list->getLength(); j++) { - if (((SoNode *)child2list->get(j))->getTypeId() == - SoVRMLIndexedFaceSet::getClassTypeId()) { + if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) { indexFaceSet *ifs = new indexFaceSet; SoVRMLIndexedFaceSet *face_set; face_set = (SoVRMLIndexedFaceSet *)child2list->get(j); @@ -199,12 +191,9 @@ void set_scene_wrl(const char *str, Bound_scene *sc, float factor) if (std::fabs(factor) > std::numeric_limits::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; + 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; } } } @@ -240,8 +229,7 @@ void extractFaces(SoVRMLIndexedFaceSet *face_set, indexFaceSet *ifs) void ifsToBound(Bound *bptr, std::list &ifs_list) { int nbPt = 0; - for (std::list::const_iterator it = ifs_list.begin(); - it != ifs_list.end(); ++it) { + for (std::list::const_iterator it = ifs_list.begin(); it != ifs_list.end(); ++it) { nbPt += (*it)->nbPt; } bptr->point.nbr = (Index)nbPt; @@ -249,8 +237,7 @@ void ifsToBound(Bound *bptr, std::list &ifs_list) ifs_list.front(); unsigned int iter = 0; - for (std::list::const_iterator it = ifs_list.begin(); - it != ifs_list.end(); ++it) { + for (std::list::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(); @@ -264,8 +251,7 @@ void ifsToBound(Bound *bptr, std::list &ifs_list) ifs_list.front(); std::list indSize; int indice = 0; - for (std::list::const_iterator it = ifs_list.begin(); - it != ifs_list.end(); ++it) { + for (std::list::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) { @@ -283,21 +269,18 @@ void ifsToBound(Bound *bptr, std::list &ifs_list) std::list::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)); + bptr->face.ptr[i].vertex.ptr = (Index *)malloc((unsigned int)*iter_indSize * sizeof(Index)); ++iter_indSize; } int offset = 0; indice = 0; - for (std::list::const_iterator it = ifs_list.begin(); - it != ifs_list.end(); ++it) { + for (std::list::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); + bptr->face.ptr[indice].vertex.ptr[iter] = (Index)(ifs->index[j] + offset); iter++; } else { iter = 0; @@ -310,17 +293,13 @@ void ifsToBound(Bound *bptr, std::list &ifs_list) void destroyIfs(std::list &ifs_list) { - for (std::list::const_iterator it = ifs_list.begin(); - it != ifs_list.end(); ++it) { + for (std::list::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*/) -{ -} +void set_scene_wrl(const char * /*str*/, Bound_scene * /*sc*/, float /*factor*/) {} #endif /* diff --git a/modules/robot/src/wireframe-simulator/vpView.h b/modules/robot/src/wireframe-simulator/vpView.h index 8a865627ca..4845ec8417 100644 --- a/modules/robot/src/wireframe-simulator/vpView.h +++ b/modules/robot/src/wireframe-simulator/vpView.h @@ -86,60 +86,57 @@ #define PARALLEL 0 #define PERSPECTIVE 1 -#define vpDEFAULT_EYE \ - { \ - 0.0, 0.0, 1.0 \ +#define vpDEFAULT_EYE \ + { \ + 0.0, 0.0, 1.0 \ } -#define vpDEFAULT_TARGET \ - { \ - 0.0, 0.0, 0.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_CAMERA \ + { \ + vpDEFAULT_EYE, vpDEFAULT_TARGET, vpDEFAULT_FOCAL, vpDEFAULT_ANGLE, vpDEFAULT_TWIST, vpDEFAULT_SPEED \ } -#define vpDEFAULT_COP \ - { \ - 0.0, 0.0, 1.0 \ +#define vpDEFAULT_COP \ + { \ + 0.0, 0.0, 1.0 \ } -#define vpDEFAULT_VRP \ - { \ - 0.0, 0.0, 0.0 \ +#define vpDEFAULT_VRP \ + { \ + 0.0, 0.0, 0.0 \ } -#define vpDEFAULT_VPN \ - { \ - 0.0, 0.0, -1.0 \ +#define vpDEFAULT_VPN \ + { \ + 0.0, 0.0, -1.0 \ } -#define vpDEFAULT_VUP \ - { \ - 0.0, 1.0, 0.0 \ +#define vpDEFAULT_VUP \ + { \ + 0.0, 1.0, 0.0 \ } -#define vpDEFAULT_VWD \ - { \ - -1.0, 1.0, -1.0, 1.0 \ +#define vpDEFAULT_VWD \ + { \ + -1.0, 1.0, -1.0, 1.0 \ } -#define vpDEFAULT_DEPTH \ - { \ - 0.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_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 \ +#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 \ } /* diff --git a/modules/robot/src/wireframe-simulator/vpVwstack.cpp b/modules/robot/src/wireframe-simulator/vpVwstack.cpp index 49019ebe49..64cb472e26 100644 --- a/modules/robot/src/wireframe-simulator/vpVwstack.cpp +++ b/modules/robot/src/wireframe-simulator/vpVwstack.cpp @@ -106,8 +106,7 @@ void fprintf_vwstack(FILE *fp, char *argv) 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); + 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; } @@ -197,8 +196,7 @@ void add_vwstack(const char *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)); + 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); @@ -223,8 +221,7 @@ void add_vwstack(const char *path, ...) } } 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)); + 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); diff --git a/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp b/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp index dd00d9c590..87ffb70721 100644 --- a/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp +++ b/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp @@ -75,9 +75,7 @@ extern Point2i *listpoint2i; /* Copy the scene corresponding to the registeresd parameters in the image. */ -void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, - const vpImage &I, - const vpColor &color) +void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage &I, const vpColor &color) { // extern Bound *clipping_Bound (); Bound *bp, *bend; @@ -98,22 +96,19 @@ void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, 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); + 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_); + 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_); + vpDisplay::displayLine(I, vpImagePoint((listpoint2i)->y, (listpoint2i)->x), vpImagePoint((pt)->y, (pt)->x), + color, thickness_); } } } @@ -124,8 +119,7 @@ void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, /* Copy the scene corresponding to the registeresd parameters in the image. */ -void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, - const vpImage &I, +void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage &I, const vpColor &color) { // extern Bound *clipping_Bound (); @@ -148,22 +142,19 @@ void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, 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); + 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_); + 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_); + vpDisplay::displayLine(I, vpImagePoint((listpoint2i)->y, (listpoint2i)->x), vpImagePoint((pt)->y, (pt)->x), + color, thickness_); } } } @@ -182,26 +173,20 @@ void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, 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() + : 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 scene_dirs = - vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";")); + std::vector 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 + if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists scene_dir = scene_dirs[i]; sceneDirExists = true; break; @@ -209,11 +194,9 @@ vpWireFrameSimulator::vpWireFrameSimulator() if (!sceneDirExists) { try { scene_dir = vpIoTools::getenv("VISP_SCENES_DIR"); - std::cout << "The simulator uses data from VISP_SCENES_DIR=" - << scene_dir << std::endl; + 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; + std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl; } } @@ -275,8 +258,7 @@ vpWireFrameSimulator::~vpWireFrameSimulator() 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) +void vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object) { char name_cam[FILENAME_MAX]; char name[FILENAME_MAX]; @@ -286,8 +268,7 @@ void vpWireFrameSimulator::initScene( 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")); + throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name")); } strcpy(name_cam, scene_dir_); @@ -374,9 +355,7 @@ void vpWireFrameSimulator::initScene( 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")); + throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the desired object name")); } switch (desiredObject) { @@ -431,9 +410,8 @@ void vpWireFrameSimulator::initScene( 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 &imObj) +void vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object, + const std::list &imObj) { initScene(obj, desired_object); objectImage = imObj; @@ -451,8 +429,7 @@ void vpWireFrameSimulator::initScene( \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) +void vpWireFrameSimulator::initScene(const char *obj, const char *desired_object) { char name_cam[FILENAME_MAX]; char name[FILENAME_MAX]; @@ -462,8 +439,7 @@ void vpWireFrameSimulator::initScene(const char *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")); + throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name")); } strcpy(name_cam, scene_dir_); @@ -471,8 +447,7 @@ void vpWireFrameSimulator::initScene(const char *obj, set_scene(name_cam, &camera, cameraFactor); if (strlen(obj) >= FILENAME_MAX) { - throw(vpException(vpException::memoryAllocationError, - "Not enough memory to intialize the name")); + throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the name")); } strcpy(name, obj); @@ -487,8 +462,7 @@ void vpWireFrameSimulator::initScene(const char *obj, } if (strlen(desired_object) >= FILENAME_MAX) { - throw(vpException(vpException::memoryAllocationError, - "Not enough memory to intialize the camera name")); + throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name")); } strcpy(name, desired_object); @@ -529,8 +503,7 @@ void vpWireFrameSimulator::initScene(const char *obj, \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, +void vpWireFrameSimulator::initScene(const char *obj, const char *desired_object, const std::list &imObj) { initScene(obj, desired_object); @@ -559,8 +532,7 @@ void vpWireFrameSimulator::initScene(const vpSceneObject &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")); + throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name")); } strcpy(name_cam, scene_dir_); @@ -673,8 +645,7 @@ void vpWireFrameSimulator::initScene(const vpSceneObject &obj) \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 &imObj) +void vpWireFrameSimulator::initScene(const vpSceneObject &obj, const std::list &imObj) { initScene(obj); objectImage = imObj; @@ -700,8 +671,7 @@ void vpWireFrameSimulator::initScene(const char *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")); + throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name")); } strcpy(name_cam, scene_dir_); @@ -709,8 +679,7 @@ void vpWireFrameSimulator::initScene(const char *obj) set_scene(name_cam, &camera, cameraFactor); if (strlen(obj) >= FILENAME_MAX) { - throw(vpException(vpException::memoryAllocationError, - "Not enough memory to intialize the name")); + throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the name")); } strcpy(name, obj); @@ -750,8 +719,7 @@ void vpWireFrameSimulator::initScene(const char *obj) \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 &imObj) +void vpWireFrameSimulator::initScene(const char *obj, const std::list &imObj) { initScene(obj); objectImage = imObj; @@ -769,23 +737,19 @@ void vpWireFrameSimulator::initScene(const char *obj, void vpWireFrameSimulator::getInternalImage(vpImage &I) { if (!sceneInitialized) - throw(vpException(vpException::notInitialized, - "The scene has to be initialized")); + 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::epsilon()) && - (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * - std::numeric_limits::epsilon())) { + if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits::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())); + v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight())); } float o44c[4][4], o44cd[4][4], x, y, z; @@ -797,8 +761,7 @@ void vpWireFrameSimulator::getInternalImage(vpImage &I) if (displayImageSimulator) { I = 255; - for (std::list::iterator it = objectImage.begin(); - it != objectImage.end(); ++it) { + for (std::list::iterator it = objectImage.begin(); it != objectImage.end(); ++it) { vpImageSimulator *imSim = &(*it); imSim->setCameraPosition(rotz * cMo); imSim->getImage(I, getInternalCameraParameters(I)); @@ -853,8 +816,7 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I) // if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] != // 0*/) if (std::fabs(displacement[2][3]) > - std::numeric_limits:: - epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/) + std::numeric_limits::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/) camMf2 = camMf2 * displacement; f2Mf = camMf2.inverse() * camMf; @@ -865,16 +827,13 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I) 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::epsilon()) && - (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * - std::numeric_limits::epsilon())) { + if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits::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())); + v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight())); } float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z; @@ -898,8 +857,7 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I) if (displayImageSimulator) { I = 255; - for (std::list::iterator it = objectImage.begin(); - it != objectImage.end(); ++it) { + for (std::list::iterator it = objectImage.begin(); it != objectImage.end(); ++it) { vpImageSimulator *imSim = &(*it); imSim->setCameraPosition(rotz * camMf * fMo); imSim->getImage(I, getInternalCameraParameters(I)); @@ -925,13 +883,10 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I) if (changed || extCamChanged) { cameraTrajectory.clear(); - std::list::const_iterator iter_poseList = - poseList.begin(); - std::list::const_iterator iter_fMoList = - fMoList.begin(); + std::list::const_iterator iter_poseList = poseList.begin(); + std::list::const_iterator iter_fMoList = fMoList.begin(); - while ((iter_poseList != poseList.end()) && - (iter_fMoList != fMoList.end())) { + while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end())) { iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList); cameraTrajectory.push_back(iP); if (camTrajType == CT_LINE) { @@ -949,9 +904,7 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I) iP = projectCameraTrajectory(I, cMo, fMo); cameraTrajectory.push_back(iP); - for (std::list::const_iterator it = - cameraTrajectory.begin(); - it != cameraTrajectory.end(); ++it) { + for (std::list::const_iterator it = cameraTrajectory.begin(); it != cameraTrajectory.end(); ++it) { if (camTrajType == CT_LINE) { if (iter != 0) vpDisplay::displayLine(I, iP_1, *it, camTrajColor, thickness_); @@ -985,8 +938,7 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I) \warning : The objects are displayed thanks to overlays. The image I is not modified. */ -void vpWireFrameSimulator::getExternalImage(vpImage &I, - const vpHomogeneousMatrix &cam_Mf) +void vpWireFrameSimulator::getExternalImage(vpImage &I, const vpHomogeneousMatrix &cam_Mf) { float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z; @@ -996,16 +948,13 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I, 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::epsilon()) && - (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * - std::numeric_limits::epsilon())) { + if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits::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())); + v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight())); } vp2jlc_matrix(camMft.inverse(), w44cext); @@ -1024,8 +973,7 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I, if (displayImageSimulator) { I = 255; - for (std::list::iterator it = objectImage.begin(); - it != objectImage.end(); ++it) { + for (std::list::iterator it = objectImage.begin(); it != objectImage.end(); ++it) { vpImageSimulator *imSim = &(*it); imSim->setCameraPosition(rotz * cam_Mf * fMo); imSim->getImage(I, getInternalCameraParameters(I)); @@ -1052,23 +1000,19 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I, void vpWireFrameSimulator::getInternalImage(vpImage &I) { if (!sceneInitialized) - throw(vpException(vpException::notInitialized, - "The scene has to be initialized")); + 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::epsilon()) && - (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * - std::numeric_limits::epsilon())) { + if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits::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())); + v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight())); } float o44c[4][4], o44cd[4][4], x, y, z; @@ -1080,8 +1024,7 @@ void vpWireFrameSimulator::getInternalImage(vpImage &I) if (displayImageSimulator) { I = 255; - for (std::list::iterator it = objectImage.begin(); - it != objectImage.end(); ++it) { + for (std::list::iterator it = objectImage.begin(); it != objectImage.end(); ++it) { vpImageSimulator *imSim = &(*it); imSim->setCameraPosition(rotz * camMf * fMo); imSim->getImage(I, getInternalCameraParameters(I)); @@ -1136,8 +1079,7 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I) // if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] != // 0*/) if (std::fabs(displacement[2][3]) > - std::numeric_limits:: - epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/) + std::numeric_limits::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/) camMf2 = camMf2 * displacement; f2Mf = camMf2.inverse() * camMf; @@ -1148,16 +1090,13 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I) 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::epsilon()) && - (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * - std::numeric_limits::epsilon())) { + if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits::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())); + v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight())); } float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z; @@ -1180,8 +1119,7 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I) if (displayImageSimulator) { I = 255; - for (std::list::iterator it = objectImage.begin(); - it != objectImage.end(); ++it) { + for (std::list::iterator it = objectImage.begin(); it != objectImage.end(); ++it) { vpImageSimulator *imSim = &(*it); imSim->setCameraPosition(rotz * camMf * fMo); imSim->getImage(I, getInternalCameraParameters(I)); @@ -1206,13 +1144,10 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I) if (changed || extCamChanged) { cameraTrajectory.clear(); - std::list::const_iterator iter_poseList = - poseList.begin(); - std::list::const_iterator iter_fMoList = - fMoList.begin(); + std::list::const_iterator iter_poseList = poseList.begin(); + std::list::const_iterator iter_fMoList = fMoList.begin(); - while ((iter_poseList != poseList.end()) && - (iter_fMoList != fMoList.end())) { + 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); @@ -1231,9 +1166,7 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I) iP = projectCameraTrajectory(I, cMo, fMo); cameraTrajectory.push_back(iP); - for (std::list::const_iterator it = - cameraTrajectory.begin(); - it != cameraTrajectory.end(); ++it) { + for (std::list::const_iterator it = cameraTrajectory.begin(); it != cameraTrajectory.end(); ++it) { if (camTrajType == CT_LINE) { if (iter != 0) vpDisplay::displayLine(I, iP_1, *it, camTrajColor, thickness_); @@ -1267,8 +1200,7 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I) \warning : The objects are displayed thanks to overlays. The image I is not modified. */ -void vpWireFrameSimulator::getExternalImage(vpImage &I, - const vpHomogeneousMatrix &cam_Mf) +void vpWireFrameSimulator::getExternalImage(vpImage &I, const vpHomogeneousMatrix &cam_Mf) { float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z; @@ -1278,16 +1210,13 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I, 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::epsilon()) && - (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * - std::numeric_limits::epsilon())) { + if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits::epsilon()) && + (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits::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())); + v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight())); } vp2jlc_matrix(camMft.inverse(), w44cext); @@ -1305,8 +1234,7 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I, if (displayImageSimulator) { I = 255; - for (std::list::iterator it = objectImage.begin(); - it != objectImage.end(); ++it) { + for (std::list::iterator it = objectImage.begin(); it != objectImage.end(); ++it) { vpImageSimulator *imSim = &(*it); imSim->setCameraPosition(rotz * cam_Mf * fMo); imSim->getImage(I, getInternalCameraParameters(I)); @@ -1338,15 +1266,13 @@ void vpWireFrameSimulator::getExternalImage(vpImage &I, of the external camera (used to project the trajectory) relative to the world reference frame. */ -void vpWireFrameSimulator::displayTrajectory( - const vpImage &I, - const std::list &list_cMo, - const std::list &list_fMo, - const vpHomogeneousMatrix &cMf) +void vpWireFrameSimulator::displayTrajectory(const vpImage &I, + const std::list &list_cMo, + const std::list &list_fMo, + const vpHomogeneousMatrix &cMf) { if (list_cMo.size() != list_fMo.size()) - throw(vpException(vpException::dimensionError, - "The two lists must have the same size")); + throw(vpException(vpException::dimensionError, "The two lists must have the same size")); vpImagePoint iP; vpImagePoint iP_1; @@ -1386,14 +1312,12 @@ void vpWireFrameSimulator::displayTrajectory( of the external camera (used to project the trajectory) relative to the world reference frame. */ -void vpWireFrameSimulator::displayTrajectory( - const vpImage &I, const std::list &list_cMo, - const std::list &list_fMo, - const vpHomogeneousMatrix &cMf) +void vpWireFrameSimulator::displayTrajectory(const vpImage &I, const std::list &list_cMo, + const std::list &list_fMo, + const vpHomogeneousMatrix &cMf) { if (list_cMo.size() != list_fMo.size()) - throw(vpException(vpException::dimensionError, - "The two lists must have the same size")); + throw(vpException(vpException::dimensionError, "The two lists must have the same size")); vpImagePoint iP; vpImagePoint iP_1; @@ -1419,8 +1343,7 @@ void vpWireFrameSimulator::displayTrajectory( /*! Enables to change the external camera position. */ -vpHomogeneousMatrix vpWireFrameSimulator::navigation(const vpImage &I, - bool &changed) +vpHomogeneousMatrix vpWireFrameSimulator::navigation(const vpImage &I, bool &changed) { double width = vpMath::minimum(I.getWidth(), I.getHeight()); vpImagePoint iP; @@ -1509,9 +1432,7 @@ vpHomogeneousMatrix vpWireFrameSimulator::navigation(const vpImage &I, /*! Enables to change the external camera position. */ -vpHomogeneousMatrix -vpWireFrameSimulator::navigation(const vpImage &I, - bool &changed) +vpHomogeneousMatrix vpWireFrameSimulator::navigation(const vpImage &I, bool &changed) { double width = vpMath::minimum(I.getWidth(), I.getHeight()); vpImagePoint iP; @@ -1602,10 +1523,8 @@ vpWireFrameSimulator::navigation(const vpImage &I, /*! Project the center of the internal camera into the external camera view. */ -vpImagePoint -vpWireFrameSimulator::projectCameraTrajectory(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpHomogeneousMatrix &fMo_) +vpImagePoint vpWireFrameSimulator::projectCameraTrajectory(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpHomogeneousMatrix &fMo_) { vpPoint point; point.setWorldCoordinates(0, 0, 0); @@ -1614,8 +1533,7 @@ vpWireFrameSimulator::projectCameraTrajectory(const vpImage &I, vpImagePoint iP; - vpMeterPixelConversion::convertPoint(getExternalCameraParameters(I), - point.get_x(), point.get_y(), iP); + vpMeterPixelConversion::convertPoint(getExternalCameraParameters(I), point.get_x(), point.get_y(), iP); return iP; } @@ -1623,10 +1541,9 @@ vpWireFrameSimulator::projectCameraTrajectory(const vpImage &I, /*! Project the center of the internal camera into the external camera view. */ -vpImagePoint -vpWireFrameSimulator::projectCameraTrajectory(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpHomogeneousMatrix &fMo_) +vpImagePoint vpWireFrameSimulator::projectCameraTrajectory(const vpImage &I, + const vpHomogeneousMatrix &cMo_, + const vpHomogeneousMatrix &fMo_) { vpPoint point; point.setWorldCoordinates(0, 0, 0); @@ -1635,8 +1552,7 @@ vpWireFrameSimulator::projectCameraTrajectory(const vpImage &I, vpImagePoint iP; - vpMeterPixelConversion::convertPoint(getExternalCameraParameters(I), - point.get_x(), point.get_y(), iP); + vpMeterPixelConversion::convertPoint(getExternalCameraParameters(I), point.get_x(), point.get_y(), iP); return iP; } @@ -1644,9 +1560,9 @@ vpWireFrameSimulator::projectCameraTrajectory(const vpImage &I, /*! Project the center of the internal camera into the external camera view. */ -vpImagePoint vpWireFrameSimulator::projectCameraTrajectory( - const vpImage &I, const vpHomogeneousMatrix &cMo_, - const vpHomogeneousMatrix &fMo_, const vpHomogeneousMatrix &cMf) +vpImagePoint vpWireFrameSimulator::projectCameraTrajectory(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpHomogeneousMatrix &fMo_, + const vpHomogeneousMatrix &cMf) { vpPoint point; point.setWorldCoordinates(0, 0, 0); @@ -1655,8 +1571,7 @@ vpImagePoint vpWireFrameSimulator::projectCameraTrajectory( vpImagePoint iP; - vpMeterPixelConversion::convertPoint(getExternalCameraParameters(I), - point.get_x(), point.get_y(), iP); + vpMeterPixelConversion::convertPoint(getExternalCameraParameters(I), point.get_x(), point.get_y(), iP); return iP; } @@ -1664,9 +1579,10 @@ vpImagePoint vpWireFrameSimulator::projectCameraTrajectory( /*! Project the center of the internal camera into the external camera view. */ -vpImagePoint vpWireFrameSimulator::projectCameraTrajectory( - const vpImage &I, const vpHomogeneousMatrix &cMo_, - const vpHomogeneousMatrix &fMo_, const vpHomogeneousMatrix &cMf) +vpImagePoint vpWireFrameSimulator::projectCameraTrajectory(const vpImage &I, + const vpHomogeneousMatrix &cMo_, + const vpHomogeneousMatrix &fMo_, + const vpHomogeneousMatrix &cMf) { vpPoint point; point.setWorldCoordinates(0, 0, 0); @@ -1675,8 +1591,7 @@ vpImagePoint vpWireFrameSimulator::projectCameraTrajectory( vpImagePoint iP; - vpMeterPixelConversion::convertPoint(getExternalCameraParameters(I), - point.get_x(), point.get_y(), iP); + vpMeterPixelConversion::convertPoint(getExternalCameraParameters(I), point.get_x(), point.get_y(), iP); return iP; } diff --git a/modules/robot/test/servo-afma4/testRobotAfma4.cpp b/modules/robot/test/servo-afma4/testRobotAfma4.cpp index cd10402623..4b093468b2 100644 --- a/modules/robot/test/servo-afma4/testRobotAfma4.cpp +++ b/modules/robot/test/servo-afma4/testRobotAfma4.cpp @@ -68,8 +68,7 @@ int main() #else int main() { - std::cout << "The real Afma4 robot controller is not available." - << std::endl; + std::cout << "The real Afma4 robot controller is not available." << std::endl; return 0; } diff --git a/modules/robot/test/servo-afma6/testAfma6.cpp b/modules/robot/test/servo-afma6/testAfma6.cpp index abbd9f035a..72e1b1a79f 100644 --- a/modules/robot/test/servo-afma6/testAfma6.cpp +++ b/modules/robot/test/servo-afma6/testAfma6.cpp @@ -64,38 +64,28 @@ int main() afma6.getCameraParameters(cam, 640, 480); std::cout << cam << std::endl; - std::cout - << "-- Settings associated to the CCMOP tool without distortion ---" - << 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 << "-- 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; + 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 << "-- 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; diff --git a/modules/robot/test/servo-afma6/testRobotAfma6.cpp b/modules/robot/test/servo-afma6/testRobotAfma6.cpp index fdd58857f3..c6b4f3b60c 100644 --- a/modules/robot/test/servo-afma6/testRobotAfma6.cpp +++ b/modules/robot/test/servo-afma6/testRobotAfma6.cpp @@ -65,37 +65,28 @@ int main() afma6.getCameraParameters(cam, 640, 480); std::cout << cam << std::endl; - std::cout - << "-- Settings associated to the CCMOP tool without distortion ---" - << 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 << "-- 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; + 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 << "-- 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; @@ -107,8 +98,7 @@ int main() #else int main() { - std::cout << "The real Afma6 robot controller is not available." - << std::endl; + std::cout << "The real Afma6 robot controller is not available." << std::endl; return 0; } diff --git a/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp b/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp index 01d5b76aad..e20c42a4b7 100644 --- a/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp +++ b/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp @@ -129,8 +129,7 @@ int main() // Load the end-effector to camera frame transformation obtained // using a camera intrinsic model with distortion - robot.init(vpAfma6::TOOL_CCMOP, - vpCameraParameters::perspectiveProjWithDistortion); + robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion); // Get the intrinsic camera parameters associated to the image robot.getCameraParameters(cam, I); @@ -173,8 +172,7 @@ int main() 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::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg" << std::endl << std::endl; @@ -195,8 +193,7 @@ int main() 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::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg" << std::endl << std::endl; @@ -207,8 +204,7 @@ int main() 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::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg" << std::endl << std::endl; @@ -217,8 +213,7 @@ int main() 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::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg" << std::endl << std::endl; @@ -230,8 +225,7 @@ int main() #else int main() { - std::cout << "Sorry, test not valid. You should have an Afma6 robot..." - << std::endl; + std::cout << "Sorry, test not valid. You should have an Afma6 robot..." << std::endl; return 0; } diff --git a/modules/robot/test/servo-viper/testRobotViper650-frames.cpp b/modules/robot/test/servo-viper/testRobotViper650-frames.cpp index be4cc0cfdd..0336510416 100644 --- a/modules/robot/test/servo-viper/testRobotViper650-frames.cpp +++ b/modules/robot/test/servo-viper/testRobotViper650-frames.cpp @@ -47,8 +47,7 @@ #ifdef VISP_HAVE_VIPER650 -bool pose_equal(const vpHomogeneousMatrix &M1, const vpHomogeneousMatrix &M2, - double epsilon = 1e-6) +bool pose_equal(const vpHomogeneousMatrix &M1, const vpHomogeneousMatrix &M2, double epsilon = 1e-6) { vpTranslationVector t1, t2; M1.extract(t1); @@ -66,8 +65,7 @@ bool pose_equal(const vpHomogeneousMatrix &M1, const vpHomogeneousMatrix &M2, return true; } -bool joint_equal(const vpColVector &q1, const vpColVector &q2, - double epsilon = 1e-6) +bool joint_equal(const vpColVector &q1, const vpColVector &q2, double epsilon = 1e-6) { for (unsigned int i = 0; i < q1.size(); i++) { if (std::fabs(q1[i] - q2[i]) > epsilon) { @@ -103,8 +101,7 @@ int main() #else // In this case, we set tool frame to the PTGrey Flea2 camera frame vpTranslationVector etc(-0.04437278107, -0.001192883711, 0.07808296844); - vpRxyzVector erxyzc(vpMath::rad(0.7226737722), vpMath::rad(2.103893926), - vpMath::rad(-90.46213439)); + vpRxyzVector erxyzc(vpMath::rad(0.7226737722), vpMath::rad(2.103893926), vpMath::rad(-90.46213439)); eMt.buildFrom(etc, vpRotationMatrix(erxyzc)); #endif std::cout << "eMt:\n" << eMt << std::endl; @@ -139,7 +136,7 @@ int main() std::cout << "eMc:\n" << cMe.inverse() << std::endl; //********* Check if retrieved eMt transformation is the one that was set - //during init + // during init if (1) { vpHomogeneousMatrix eMt_ = fMe.inverse() * fMt; std::cout << "eMt_:\n" << eMt_ << std::endl; @@ -154,7 +151,7 @@ int main() std::cout << " They are the same, we can continue" << std::endl; //********* Check if retrieved eMc transformation is the one that was - //set + // set std::cout << "eMc:\n" << cMe.inverse() << std::endl; // Compare pose @@ -195,8 +192,7 @@ int main() vpColVector q1; robot.getInverseKinematics(fMt, q1); - std::cout << "Move robot in joint (the robot should not move)" - << std::endl; + std::cout << "Move robot in joint (the robot should not move)" << std::endl; robot.setRobotState(vpRobot::STATE_POSITION_CONTROL); robot.setPosition(vpRobotViper650::ARTICULAR_FRAME, q1); @@ -223,8 +219,7 @@ int main() f_pose_t[i + 3] = f_rxyz_t[i]; } - std::cout << "Move robot in reference frame (the robot should not move)" - << std::endl; + std::cout << "Move robot in reference frame (the robot should not move)" << std::endl; robot.setPosition(vpRobot::REFERENCE_FRAME, f_pose_t); vpColVector q3; robot.getPosition(vpRobot::ARTICULAR_FRAME, q3); @@ -372,16 +367,14 @@ int main() std::cout << "The end" << std::endl; std::cout << "Test succeed" << std::endl; } catch (vpException &e) { - std::cout << "Test failed with exception: " << e.getMessage() - << std::endl; + std::cout << "Test failed with exception: " << e.getMessage() << std::endl; } } #else int main() { - std::cout << "The real Viper650 robot controller is not available." - << std::endl; + std::cout << "The real Viper650 robot controller is not available." << std::endl; return 0; } diff --git a/modules/robot/test/servo-viper/testRobotViper850-frames.cpp b/modules/robot/test/servo-viper/testRobotViper850-frames.cpp index 43315ec578..b1d61cfb58 100644 --- a/modules/robot/test/servo-viper/testRobotViper850-frames.cpp +++ b/modules/robot/test/servo-viper/testRobotViper850-frames.cpp @@ -47,8 +47,7 @@ #ifdef VISP_HAVE_VIPER850 -bool pose_equal(const vpHomogeneousMatrix &M1, const vpHomogeneousMatrix &M2, - double epsilon = 1e-6) +bool pose_equal(const vpHomogeneousMatrix &M1, const vpHomogeneousMatrix &M2, double epsilon = 1e-6) { vpTranslationVector t1, t2; M1.extract(t1); @@ -66,8 +65,7 @@ bool pose_equal(const vpHomogeneousMatrix &M1, const vpHomogeneousMatrix &M2, return true; } -bool joint_equal(const vpColVector &q1, const vpColVector &q2, - double epsilon = 1e-6) +bool joint_equal(const vpColVector &q1, const vpColVector &q2, double epsilon = 1e-6) { for (unsigned int i = 0; i < q1.size(); i++) { if (std::fabs(q1[i] - q2[i]) > epsilon) { @@ -103,8 +101,7 @@ int main() #else // In this case, we set tool frame to the PTGrey Flea2 camera frame vpTranslationVector etc(-0.04437278107, -0.001192883711, 0.07808296844); - vpRxyzVector erxyzc(vpMath::rad(0.7226737722), vpMath::rad(2.103893926), - vpMath::rad(-90.46213439)); + vpRxyzVector erxyzc(vpMath::rad(0.7226737722), vpMath::rad(2.103893926), vpMath::rad(-90.46213439)); eMt.buildFrom(etc, vpRotationMatrix(erxyzc)); #endif std::cout << "eMt:\n" << eMt << std::endl; @@ -139,7 +136,7 @@ int main() std::cout << "eMc:\n" << cMe.inverse() << std::endl; //********* Check if retrieved eMt transformation is the one that was set - //during init + // during init if (1) { vpHomogeneousMatrix eMt_ = fMe.inverse() * fMt; std::cout << "eMt_:\n" << eMt_ << std::endl; @@ -154,7 +151,7 @@ int main() std::cout << " They are the same, we can continue" << std::endl; //********* Check if retrieved eMc transformation is the one that was - //set + // set std::cout << "eMc:\n" << cMe.inverse() << std::endl; // Compare pose @@ -195,8 +192,7 @@ int main() vpColVector q1; robot.getInverseKinematics(fMt, q1); - std::cout << "Move robot in joint (the robot should not move)" - << std::endl; + std::cout << "Move robot in joint (the robot should not move)" << std::endl; robot.setRobotState(vpRobot::STATE_POSITION_CONTROL); robot.setPosition(vpRobotViper850::ARTICULAR_FRAME, q1); @@ -223,8 +219,7 @@ int main() f_pose_t[i + 3] = f_rxyz_t[i]; } - std::cout << "Move robot in reference frame (the robot should not move)" - << std::endl; + std::cout << "Move robot in reference frame (the robot should not move)" << std::endl; robot.setPosition(vpRobot::REFERENCE_FRAME, f_pose_t); vpColVector q3; robot.getPosition(vpRobot::ARTICULAR_FRAME, q3); @@ -372,16 +367,14 @@ int main() std::cout << "The end" << std::endl; std::cout << "Test succeed" << std::endl; } catch (vpException &e) { - std::cout << "Test failed with exception: " << e.getMessage() - << std::endl; + std::cout << "Test failed with exception: " << e.getMessage() << std::endl; } } #else int main() { - std::cout << "The real Viper850 robot controller is not available." - << std::endl; + std::cout << "The real Viper850 robot controller is not available." << std::endl; return 0; } diff --git a/modules/robot/test/servo-viper/testRobotViper850.cpp b/modules/robot/test/servo-viper/testRobotViper850.cpp index 82903e5b58..dccd30ed86 100644 --- a/modules/robot/test/servo-viper/testRobotViper850.cpp +++ b/modules/robot/test/servo-viper/testRobotViper850.cpp @@ -76,8 +76,7 @@ int main() std::cout << "-- Settings associated to the Marlin F033C camera with " "distortion ------" << std::endl; - viper850.init(vpViper850::TOOL_MARLIN_F033C_CAMERA, - vpCameraParameters::perspectiveProjWithDistortion); + viper850.init(vpViper850::TOOL_MARLIN_F033C_CAMERA, vpCameraParameters::perspectiveProjWithDistortion); std::cout << viper850 << std::endl; viper850.getCameraParameters(cam, 640, 480); std::cout << cam << std::endl; @@ -100,8 +99,7 @@ int main() std::cout << "fMe:" << std::endl << "\tt: " << t.t() << std::endl - << "\trzyz (deg): " << vpMath::deg(rzyz[0]) << " " - << vpMath::deg(rzyz[1]) << " " << vpMath::deg(rzyz[2]) + << "\trzyz (deg): " << vpMath::deg(rzyz[0]) << " " << vpMath::deg(rzyz[1]) << " " << vpMath::deg(rzyz[2]) << std::endl; } catch (vpException &e) { @@ -112,8 +110,7 @@ int main() #else int main() { - std::cout << "The real Viper850 robot controller is not available." - << std::endl; + std::cout << "The real Viper850 robot controller is not available." << std::endl; return 0; } diff --git a/modules/robot/test/servo-viper/testRobotViper850Pose.cpp b/modules/robot/test/servo-viper/testRobotViper850Pose.cpp index 41aaa141df..b12e3c7482 100644 --- a/modules/robot/test/servo-viper/testRobotViper850Pose.cpp +++ b/modules/robot/test/servo-viper/testRobotViper850Pose.cpp @@ -131,8 +131,7 @@ int main() // Load the end-effector to camera frame transformation obtained // using a camera intrinsic model with distortion - robot.init(vpViper850::defaultTool, - vpCameraParameters::perspectiveProjWithDistortion); + robot.init(vpViper850::defaultTool, vpCameraParameters::perspectiveProjWithDistortion); // Get the intrinsic camera parameters associated to the image robot.getCameraParameters(cam, I); @@ -175,8 +174,7 @@ int main() 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::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg" << std::endl << std::endl; @@ -197,8 +195,7 @@ int main() 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::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg" << std::endl << std::endl; @@ -209,8 +206,7 @@ int main() 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::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg" << std::endl << std::endl; @@ -219,8 +215,7 @@ int main() 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::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg" << std::endl << std::endl; @@ -232,8 +227,7 @@ int main() #else int main() { - std::cout << "Sorry, test not valid. You should have an Viper850 robot..." - << std::endl; + std::cout << "Sorry, test not valid. You should have an Viper850 robot..." << std::endl; return 0; } diff --git a/modules/robot/test/servo-viper/testViper650.cpp b/modules/robot/test/servo-viper/testViper650.cpp index d85b943cf8..3ed74281d3 100644 --- a/modules/robot/test/servo-viper/testViper650.cpp +++ b/modules/robot/test/servo-viper/testViper650.cpp @@ -75,8 +75,7 @@ int main() std::cout << "-- Settings associated to the Marlin F033C camera with " "distortion ------" << std::endl; - viper650.init(vpViper650::TOOL_MARLIN_F033C_CAMERA, - vpCameraParameters::perspectiveProjWithDistortion); + viper650.init(vpViper650::TOOL_MARLIN_F033C_CAMERA, vpCameraParameters::perspectiveProjWithDistortion); std::cout << viper650 << std::endl; viper650.getCameraParameters(cam, 640, 480); std::cout << cam << std::endl; @@ -110,8 +109,8 @@ int main() 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; + << "\trzyz (deg): " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) + << std::endl; return 0; } catch (vpException &e) { diff --git a/modules/robot/test/servo-viper/testViper850.cpp b/modules/robot/test/servo-viper/testViper850.cpp index 2634945a37..9602ef7554 100644 --- a/modules/robot/test/servo-viper/testViper850.cpp +++ b/modules/robot/test/servo-viper/testViper850.cpp @@ -75,8 +75,7 @@ int main() std::cout << "-- Settings associated to the Marlin F033C camera with " "distortion ------" << std::endl; - viper850.init(vpViper850::TOOL_MARLIN_F033C_CAMERA, - vpCameraParameters::perspectiveProjWithDistortion); + viper850.init(vpViper850::TOOL_MARLIN_F033C_CAMERA, vpCameraParameters::perspectiveProjWithDistortion); std::cout << viper850 << std::endl; viper850.getCameraParameters(cam, 640, 480); std::cout << cam << std::endl; @@ -110,8 +109,8 @@ int main() 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; + << "\trzyz (deg): " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) + << std::endl; return 0; } catch (vpException &e) { diff --git a/modules/robot/test/virtuose/testVirtuose.cpp b/modules/robot/test/virtuose/testVirtuose.cpp index 82c07c7a4f..05feb2c1e9 100644 --- a/modules/robot/test/virtuose/testVirtuose.cpp +++ b/modules/robot/test/virtuose/testVirtuose.cpp @@ -64,7 +64,6 @@ int main() std::cout << "Catch an exception: " << e.getStringMessage() << std::endl; } #else - std::cout << "You should install Virtuose SDK to use this binary..." - << std::endl; + std::cout << "You should install Virtuose SDK to use this binary..." << std::endl; #endif } diff --git a/modules/robot/test/virtuose/testVirtuoseAfma6.cpp b/modules/robot/test/virtuose/testVirtuoseAfma6.cpp index 4fce64f1e8..81a685bc33 100644 --- a/modules/robot/test/virtuose/testVirtuoseAfma6.cpp +++ b/modules/robot/test/virtuose/testVirtuoseAfma6.cpp @@ -81,8 +81,7 @@ int main() // Set the extrinsic camera parameters obtained with a perpective // projection model including a distortion parameter - robot.init(vpAfma6::TOOL_CCMOP, - vpCameraParameters::perspectiveProjWithDistortion); + robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion); // Initialize the controller to position control robot.setRobotState(vpRobot::STATE_POSITION_CONTROL); // Moves the robot in the joint space @@ -110,20 +109,17 @@ int main() for (int i = 0; i < 3; i++) { if (robot_cart_position[i] >= max[i]) { - force_feedback_robot[i] = - (max[i] - robot_cart_position[i]) * force_increase_rate; + force_feedback_robot[i] = (max[i] - robot_cart_position[i]) * force_increase_rate; if (force_feedback_robot[i] <= -force_limit) force_feedback_robot[i] = -force_limit; } else if (robot_cart_position[i] <= min[i]) { - force_feedback_robot[i] = - (min[i] - robot_cart_position[i]) * force_increase_rate; + force_feedback_robot[i] = (min[i] - robot_cart_position[i]) * force_increase_rate; if (force_feedback_robot[i] >= force_limit) force_feedback_robot[i] = force_limit; } else force_feedback_robot[i] = 0; } - vpColVector force_feedback_virt = - rMv.getRotationMatrix().inverse() * force_feedback_robot; + vpColVector force_feedback_virt = rMv.getRotationMatrix().inverse() * force_feedback_robot; // Printing force feedback std::cout << "Force feedback: " << force_feedback_virt.t() << std::endl; @@ -147,7 +143,6 @@ int main() std::cout << "Catch an exception: " << e.getStringMessage() << std::endl; } #else - std::cout << "You should install Virtuose SDK to use this binary..." - << std::endl; + std::cout << "You should install Virtuose SDK to use this binary..." << std::endl; #endif } diff --git a/modules/robot/test/virtuose/testVirtuoseHapticBox.cpp b/modules/robot/test/virtuose/testVirtuoseHapticBox.cpp index 62f059f3a6..0bcef6f42d 100644 --- a/modules/robot/test/virtuose/testVirtuoseHapticBox.cpp +++ b/modules/robot/test/virtuose/testVirtuoseHapticBox.cpp @@ -72,10 +72,8 @@ void CallBackVirtuose(VirtContext VC, void *ptr) // Estimated Inertia1 = m*l*l/12 // Estimated Inertia2 = m*l*l/2 (rotation w.r.t. e-e z axis) double virtualStiffnessAng = 20; - double virtualDamperAng = - 0.182; // greater than sqrt 4*Inertia1*virtualStiffnessAng - double virtualDamperAng2 = - 0.0456; // greater than sqrt 4*Inertia2*virtualStiffnessAng + double virtualDamperAng = 0.182; // greater than sqrt 4*Inertia1*virtualStiffnessAng + double virtualDamperAng2 = 0.0456; // greater than sqrt 4*Inertia2*virtualStiffnessAng vpColVector xd(3, 0); vpColVector yd(3, 0); @@ -172,8 +170,7 @@ void CallBackVirtuose(VirtContext VC, void *ptr) vpColVector rotzYZ(3, 0); rotzYZ = zeed.skew(zeed) * zYZ.normalize(); vpColVector forceStiff1 = virtualStiffnessAng * rotzYZ; - vpColVector forceDamp1 = - virtualDamperAng * (omegad * rotzYZ.normalize()) * rotzYZ.normalize(); + vpColVector forceDamp1 = virtualDamperAng * (omegad * rotzYZ.normalize()) * rotzYZ.normalize(); for (unsigned int i = 0; i < 3; i++) torque1[i] = forceStiff1[i] - forceDamp1[i]; @@ -184,8 +181,7 @@ void CallBackVirtuose(VirtContext VC, void *ptr) vpColVector rotzXZ(3, 0); rotzXZ = zeed.skew(zeed) * zXZ.normalize(); vpColVector forceStiff2 = virtualStiffnessAng * rotzXZ; - vpColVector forceDamp2 = - virtualDamperAng * (omegad * rotzXZ.normalize()) * rotzXZ.normalize(); + vpColVector forceDamp2 = virtualDamperAng * (omegad * rotzXZ.normalize()) * rotzXZ.normalize(); for (unsigned int i = 0; i < 3; i++) torque2[i] = forceStiff2[i] - forceDamp2[i]; diff --git a/modules/robot/test/virtuose/testVirtuoseJointLimits.cpp b/modules/robot/test/virtuose/testVirtuoseJointLimits.cpp index 25b5f366a5..8f424a65d6 100644 --- a/modules/robot/test/virtuose/testVirtuoseJointLimits.cpp +++ b/modules/robot/test/virtuose/testVirtuoseJointLimits.cpp @@ -54,10 +54,8 @@ void CallBackVirtuose(VirtContext VC, void *ptr) (void)VC; vpVirtuose *p_virtuose = (vpVirtuose *)ptr; - float maxQ[6] = {0.7811045051f, -0.07668215036f, 2.481732368f, - 2.819076777f, 1.044736624f, 2.687076807f}; - float minQ[6] = {-0.8011951447f, -1.648244739f, 0.7439950705f, - -3.022218227f, -1.260564089f, -2.054088593f}; + float maxQ[6] = {0.7811045051f, -0.07668215036f, 2.481732368f, 2.819076777f, 1.044736624f, 2.687076807f}; + float minQ[6] = {-0.8011951447f, -1.648244739f, 0.7439950705f, -3.022218227f, -1.260564089f, -2.054088593f}; unsigned int numJoint = 6; vpColVector feedbackRegion(numJoint, 0); @@ -75,22 +73,12 @@ void CallBackVirtuose(VirtContext VC, void *ptr) for (unsigned int iter = 0; iter < numJoint; iter++) { if (currentQ[iter] >= (maxQ[iter] - feedbackRegion[iter])) { forceFeedback[iter] = - -saturationForce[iter] * - pow((currentQ[iter] - maxQ[iter] + feedbackRegion[iter]) / - feedbackRegion[iter], - 2); - std::cout - << "WARNING! Getting close to the maximum joint limit. Joint #" - << iter + 1 << std::endl; + -saturationForce[iter] * pow((currentQ[iter] - maxQ[iter] + feedbackRegion[iter]) / feedbackRegion[iter], 2); + std::cout << "WARNING! Getting close to the maximum joint limit. Joint #" << iter + 1 << std::endl; } else if (currentQ[iter] <= (minQ[iter] + feedbackRegion[iter])) { forceFeedback[iter] = - saturationForce[iter] * - pow((minQ[iter] + feedbackRegion[iter] - currentQ[iter]) / - feedbackRegion[iter], - 2); - std::cout - << "WARNING! Getting close to the minimum joint limit. Joint #" - << iter + 1 << std::endl; + saturationForce[iter] * pow((minQ[iter] + feedbackRegion[iter] - currentQ[iter]) / feedbackRegion[iter], 2); + std::cout << "WARNING! Getting close to the minimum joint limit. Joint #" << iter + 1 << std::endl; } else { forceFeedback[iter] = 0; std::cout << "Safe zone" << std::endl; @@ -176,9 +164,5 @@ int main() } #else -int main() -{ - std::cout << "You should install Virtuose API to use this binary..." - << std::endl; -} +int main() { std::cout << "You should install Virtuose API to use this binary..." << std::endl; } #endif diff --git a/modules/robot/test/virtuose/testVirtuosePeriodicFunction.cpp b/modules/robot/test/virtuose/testVirtuosePeriodicFunction.cpp index 8c63a9058d..c3926dd9f9 100644 --- a/modules/robot/test/virtuose/testVirtuosePeriodicFunction.cpp +++ b/modules/robot/test/virtuose/testVirtuosePeriodicFunction.cpp @@ -86,9 +86,5 @@ int main() } #else -int main() -{ - std::cout << "You should install Virtuose API to use this binary..." - << std::endl; -} +int main() { std::cout << "You should install Virtuose API to use this binary..." << std::endl; } #endif diff --git a/modules/sensor/include/visp3/sensor/vp1394CMUGrabber.h b/modules/sensor/include/visp3/sensor/vp1394CMUGrabber.h index 179d5e9461..1eada0b721 100644 --- a/modules/sensor/include/visp3/sensor/vp1394CMUGrabber.h +++ b/modules/sensor/include/visp3/sensor/vp1394CMUGrabber.h @@ -108,11 +108,11 @@ int main() 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; + 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); @@ -126,8 +126,7 @@ std::endl; // Display camera description g.displayCameraDescription(0); g.displayCameraModel(); - std::cout << "Height: " << g.getHeight() << " Width: " << g.getWidth() << -std::endl; + std::cout << "Height: " << g.getHeight() << " Width: " << g.getWidth() << std::endl; vpDisplayOpenCV d(I); vpDisplay::display(I); @@ -155,15 +154,7 @@ class VISP_EXPORT vp1394CMUGrabber : public vpFrameGrabber /*! Enumeration of color codings. */ - typedef enum { - YUV444, - YUV422, - YUV411, - RGB8, - MONO8, - MONO16, - UNKNOWN - } vpColorCodingType; + typedef enum { YUV444, YUV422, YUV411, RGB8, MONO8, MONO16, UNKNOWN } vpColorCodingType; private: //! Current camera handle diff --git a/modules/sensor/include/visp3/sensor/vp1394TwoGrabber.h b/modules/sensor/include/visp3/sensor/vp1394TwoGrabber.h index 9d07773903..a154b67218 100644 --- a/modules/sensor/include/visp3/sensor/vp1394TwoGrabber.h +++ b/modules/sensor/include/visp3/sensor/vp1394TwoGrabber.h @@ -87,17 +87,19 @@ 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 #include - #include #include - +specific camera by its GUID: +\code +#include +#include +#include +#include int main() { #if defined(VISP_HAVE_DC1394) vpImage 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 + 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(); @@ -125,17 +127,20 @@ party lib \endcode - Here an example of multi camera capture. An other example is available in -setCamera(): \code #include #include - #include #include - +setCamera(): +\code +#include +#include +#include +#include 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(); + 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 *I = new vpImage [ncameras]; @@ -365,11 +370,9 @@ class VISP_EXPORT vp1394TwoGrabber : public vpFrameGrabber dc1394video_frame_t *dequeue(); dc1394video_frame_t *dequeue(vpImage &I); - dc1394video_frame_t *dequeue(vpImage &I, uint64_t ×tamp, - uint32_t &id); + dc1394video_frame_t *dequeue(vpImage &I, uint64_t ×tamp, uint32_t &id); dc1394video_frame_t *dequeue(vpImage &I); - dc1394video_frame_t *dequeue(vpImage &I, uint64_t ×tamp, - uint32_t &id); + dc1394video_frame_t *dequeue(vpImage &I, uint64_t ×tamp, uint32_t &id); void enqueue(dc1394video_frame_t *frame); static std::string framerate2string(vp1394TwoFramerateType fps); @@ -380,12 +383,9 @@ class VISP_EXPORT vp1394TwoGrabber : public vpFrameGrabber uint64_t getCamera(); void getCamera(uint64_t &camera); void getColorCoding(vp1394TwoColorCodingType &coding); - uint32_t - getColorCodingSupported(vp1394TwoVideoModeType videomode, - std::list &codings); + uint32_t getColorCodingSupported(vp1394TwoVideoModeType videomode, std::list &codings); void getFramerate(vp1394TwoFramerateType &fps); - uint32_t getFramerateSupported(vp1394TwoVideoModeType videomode, - std::list &fps); + uint32_t getFramerateSupported(vp1394TwoVideoModeType videomode, std::list &fps); uint64_t getGuid(); void getGuid(uint64_t &guid); void getHeight(unsigned int &height); @@ -395,16 +395,13 @@ class VISP_EXPORT vp1394TwoGrabber : public vpFrameGrabber unsigned int getParameterValue(vp1394TwoParametersType param); unsigned int getRingBufferSize() const; void getVideoMode(vp1394TwoVideoModeType &videomode); - uint32_t - getVideoModeSupported(std::list &videomodes); + uint32_t getVideoModeSupported(std::list &videomodes); void getWidth(unsigned int &width); unsigned int getWidth(); bool isColor(); - bool isColorCodingSupported(vp1394TwoVideoModeType videomode, - vp1394TwoColorCodingType coding); - bool isFramerateSupported(vp1394TwoVideoModeType videomode, - vp1394TwoFramerateType fps); + bool isColorCodingSupported(vp1394TwoVideoModeType videomode, vp1394TwoColorCodingType coding); + bool isFramerateSupported(vp1394TwoVideoModeType videomode, vp1394TwoFramerateType fps); bool isVideoModeSupported(vp1394TwoVideoModeType videomode); bool isVideoModeFormat7(vp1394TwoVideoModeType videomode); @@ -424,8 +421,7 @@ class VISP_EXPORT vp1394TwoGrabber : public vpFrameGrabber 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 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); diff --git a/modules/sensor/include/visp3/sensor/vpComedi.h b/modules/sensor/include/visp3/sensor/vpComedi.h index 50e01cb625..b49b98aeba 100644 --- a/modules/sensor/include/visp3/sensor/vpComedi.h +++ b/modules/sensor/include/visp3/sensor/vpComedi.h @@ -86,8 +86,10 @@ this group. To this end: 5. Reboot The following example shows how to run an synchronous data acquisition at -500 Hz, calling getPhyData() each 2 ms: \code #include - +500 Hz, calling getPhyData() each 2 ms: + +\code +#include int main() { @@ -97,8 +99,8 @@ int main() comedi.open(); for(unsigned int i=0; i < 500; i++) { - std::cout << "Physical data (in " << comedi.getPhyDataUnits() << "): " << -comedi.getPhyData().t() << std::endl; vpTime::wait(2); + std::cout << "Physical data (in " << comedi.getPhyDataUnits() << "): " << comedi.getPhyData().t() << std::endl; + vpTime::wait(2); } comedi.close(); } @@ -143,10 +145,7 @@ class VISP_EXPORT vpComedi Number of channels to read from sensor. For a 6-dim force/torque sensor use 6. */ - void setChannelNumbers(const unsigned int &nchannel) - { - m_nchannel = nchannel; - } + void setChannelNumbers(const unsigned int &nchannel) { m_nchannel = nchannel; } //! Set comedi device name. Default value is /dev/comedi0. void setDevice(const std::string &device) { m_device = device; } @@ -158,10 +157,7 @@ class VISP_EXPORT vpComedi */ void setRange(const unsigned int &range) { m_range = range; } //! Set comedi analog input subdevice. - void setSubDevice(const unsigned int &subdevice) - { - m_subdevice = subdevice; - } + void setSubDevice(const unsigned int &subdevice) { m_subdevice = subdevice; } //@} protected: @@ -170,12 +166,12 @@ class VISP_EXPORT vpComedi protected: //! @name Protected Member Functions Inherited from vpComedi //@{ - std::string m_device; /*!< Comedi device name (default: /dev/comedi0) */ - comedi_t *m_handler; /*!< Comedi handler */ - unsigned int m_subdevice; /*!< Input subdevice */ - unsigned int m_range; /*!< Range of a channel */ - unsigned int m_aref; /*!< Analog reference */ - unsigned int m_nchannel; /*!< Number of channels */ + std::string m_device; /*!< Comedi device name (default: /dev/comedi0) */ + comedi_t *m_handler; /*!< Comedi handler */ + unsigned int m_subdevice; /*!< Input subdevice */ + unsigned int m_range; /*!< Range of a channel */ + unsigned int m_aref; /*!< Analog reference */ + unsigned int m_nchannel; /*!< Number of channels */ std::vector m_range_info; /*!< Range information */ std::vector m_maxdata; /*!< Max data value */ std::vector m_chanlist; /*!< Channel list */ diff --git a/modules/sensor/include/visp3/sensor/vpDirectShowDevice.h b/modules/sensor/include/visp3/sensor/vpDirectShowDevice.h index 227a71afd2..4f821e0748 100644 --- a/modules/sensor/include/visp3/sensor/vpDirectShowDevice.h +++ b/modules/sensor/include/visp3/sensor/vpDirectShowDevice.h @@ -60,10 +60,7 @@ class VISP_EXPORT vpDirectShowDevice public: vpDirectShowDevice() : inUse(false) {} - explicit vpDirectShowDevice(const CComPtr &moniker) : inUse(false) - { - init(moniker); - } + explicit vpDirectShowDevice(const CComPtr &moniker) : inUse(false) { init(moniker); } bool init(const CComPtr &moniker); @@ -77,8 +74,7 @@ class VISP_EXPORT vpDirectShowDevice bool operator==(vpDirectShowDevice &dev); - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - vpDirectShowDevice &dev); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpDirectShowDevice &dev); }; #endif #endif diff --git a/modules/sensor/include/visp3/sensor/vpDirectShowGrabber.h b/modules/sensor/include/visp3/sensor/vpDirectShowGrabber.h index 20cf50d1d6..583fb2a2bc 100644 --- a/modules/sensor/include/visp3/sensor/vpDirectShowGrabber.h +++ b/modules/sensor/include/visp3/sensor/vpDirectShowGrabber.h @@ -99,8 +99,7 @@ class VISP_EXPORT vpDirectShowGrabber : public vpFrameGrabber bool setFormat(unsigned int width, unsigned int height, double framerate); // get capture format - void getFormat(unsigned int &width, unsigned int &height, - double &framerate); + void getFormat(unsigned int &width, unsigned int &height, double &framerate); // Get the available capture formats bool getStreamCapabilities(); diff --git a/modules/sensor/include/visp3/sensor/vpDirectShowGrabberImpl.h b/modules/sensor/include/visp3/sensor/vpDirectShowGrabberImpl.h index bb2c4b59dd..27efe7ab91 100644 --- a/modules/sensor/include/visp3/sensor/vpDirectShowGrabberImpl.h +++ b/modules/sensor/include/visp3/sensor/vpDirectShowGrabberImpl.h @@ -132,8 +132,7 @@ Enumeration of video subtypes. bool setFormat(unsigned int width, unsigned int height, double framerate); // get capture format - void getFormat(unsigned int &width, unsigned int &height, - double &framerate); + void getFormat(unsigned int &width, unsigned int &height, double &framerate); // set capture MediaType bool setMediaType(int mediaTypeID); @@ -147,27 +146,23 @@ Enumeration of video subtypes. private: CComPtr pGraph; // our DS filter graph - CComPtr - pBuild; // the interface to the capture graph builder - // used to build the filter graph + CComPtr pBuild; // the interface to the capture graph builder + // used to build the filter graph CComPtr pCapSource; // the capture source filter - CComPtr - pGrabberI; // the sample grabber's interface and filter + CComPtr pGrabberI; // the sample grabber's interface and filter CComPtr pGrabberFilter; CComPtr pControl; // The DS filter graph control interface CComPtr pEvent; // The DS filter graph event interface - vpDirectShowSampleGrabberI - sgCB; // Interface used to implement the frame grabber callback + 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 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 @@ -182,8 +177,7 @@ Enumeration of video subtypes. // selects a random video input from the enumeration and returns the // associated filter - bool selectRandomSource(CComPtr &ppVideoInputEnum, - CComPtr &pCapSource); + bool selectRandomSource(CComPtr &ppVideoInputEnum, CComPtr &pCapSource); // creates the filter graph bool createGraph(); @@ -195,8 +189,7 @@ Enumeration of video subtypes. bool checkSourceType(CComPtr &pCapSourcePin); // connects the filters as needed - bool connectSourceToGrabber(CComPtr &pCapSource, - CComPtr &pGrabberFilter); + bool connectSourceToGrabber(CComPtr &pCapSource, CComPtr &pGrabberFilter); // used to convert HRESULT-associated error message to a string void HRtoStr(std::string str); diff --git a/modules/sensor/include/visp3/sensor/vpDirectShowSampleGrabberI.h b/modules/sensor/include/visp3/sensor/vpDirectShowSampleGrabberI.h index bb55e090b9..50fa81efd9 100644 --- a/modules/sensor/include/visp3/sensor/vpDirectShowSampleGrabberI.h +++ b/modules/sensor/include/visp3/sensor/vpDirectShowSampleGrabberI.h @@ -67,10 +67,7 @@ class VISP_EXPORT vpDirectShowSampleGrabberI : public ISampleGrabberCB STDMETHODIMP QueryInterface(REFIID riid, void **ppvObject); // not implemented - STDMETHODIMP SampleCB(double Time, IMediaSample *pSample) - { - return E_NOTIMPL; - } + STDMETHODIMP SampleCB(double Time, IMediaSample *pSample) { return E_NOTIMPL; } // our callback function STDMETHODIMP BufferCB(double Time, BYTE *pBuffer, long BufferLen); diff --git a/modules/sensor/include/visp3/sensor/vpFlyCaptureGrabber.h b/modules/sensor/include/visp3/sensor/vpFlyCaptureGrabber.h index fd4755d42e..091b0ba6c7 100644 --- a/modules/sensor/include/visp3/sensor/vpFlyCaptureGrabber.h +++ b/modules/sensor/include/visp3/sensor/vpFlyCaptureGrabber.h @@ -88,8 +88,7 @@ int main() vpImage I; char filename[255]; vpFlyCaptureGrabber g; - std::cout << "Number of cameras detected: " << g.getNumCameras() << -std::endl; + std::cout << "Number of cameras detected: " << g.getNumCameras() << std::endl; g.setCameraIndex(0); // Default camera is the first on the bus g.getCameraInfo(std::cout); @@ -161,10 +160,9 @@ class VISP_EXPORT vpFlyCaptureGrabber : public vpFrameGrabber void disconnect(); float getBrightness(); - std::ostream & - getCameraInfo(std::ostream &os); // Cannot be const since - // FlyCapture2::Camera::GetCameraInfo() - // isn't + std::ostream &getCameraInfo(std::ostream &os); // Cannot be const since + // FlyCapture2::Camera::GetCameraInfo() + // isn't FlyCapture2::Camera *getCameraHandler(); /*! Return the index of the active camera. */ unsigned int getCameraIndex() const { return m_index; }; @@ -183,8 +181,7 @@ class VISP_EXPORT vpFlyCaptureGrabber : public vpFrameGrabber //! Return true if the camera capture is started. bool isCaptureStarted() const { return m_capture; } bool isFormat7Supported(FlyCapture2::Mode format7_mode); - bool isVideoModeAndFrameRateSupported(FlyCapture2::VideoMode video_mode, - FlyCapture2::FrameRate frame_rate); + bool isVideoModeAndFrameRateSupported(FlyCapture2::VideoMode video_mode, FlyCapture2::FrameRate frame_rate); void open(vpImage &I); void open(vpImage &I); @@ -195,18 +192,14 @@ class VISP_EXPORT vpFlyCaptureGrabber : public vpFrameGrabber void setCameraIndex(unsigned int index); void setCameraPower(bool on); void setCameraSerial(unsigned int serial); - float setExposure(bool exposure_on, bool exposure_auto, - float exposure_value = 0); + float setExposure(bool exposure_on, bool exposure_auto, float exposure_value = 0); float setGain(bool gain_auto, float gain_value = 0); - void setFormat7VideoMode(FlyCapture2::Mode format7_mode, - FlyCapture2::PixelFormat pixel_format, - unsigned int width, unsigned int height); + void setFormat7VideoMode(FlyCapture2::Mode format7_mode, FlyCapture2::PixelFormat pixel_format, unsigned int width, + unsigned int height); float setFrameRate(float frame_rate); - unsigned int setSharpness(bool sharpness_on, bool sharpness_auto, - unsigned int sharpness_value = 0); + unsigned int setSharpness(bool sharpness_on, bool sharpness_auto, unsigned int sharpness_value = 0); float setShutter(bool auto_shutter, float shutter_ms = 10); - void setVideoModeAndFrameRate(FlyCapture2::VideoMode video_mode, - FlyCapture2::FrameRate frame_rate); + void setVideoModeAndFrameRate(FlyCapture2::VideoMode video_mode, FlyCapture2::FrameRate frame_rate); void startCapture(); void stopCapture(); @@ -216,14 +209,11 @@ class VISP_EXPORT vpFlyCaptureGrabber : public vpFrameGrabber ABS_VALUE, //!< Consider FlyCapture2::Property::absValue VALUE_A, //!< Consider FlyCapture2::Property::valueA } PropertyValue; - std::pair - centerRoi(unsigned int size, unsigned int max_size, unsigned int step); + std::pair centerRoi(unsigned int size, unsigned int max_size, unsigned int step); FlyCapture2::Property getProperty(FlyCapture2::PropertyType prop_type); - FlyCapture2::PropertyInfo - getPropertyInfo(FlyCapture2::PropertyType prop_type); + FlyCapture2::PropertyInfo getPropertyInfo(FlyCapture2::PropertyType prop_type); void open(); - void setProperty(const FlyCapture2::PropertyType &prop_type, bool on, - bool auto_on, float value, + void setProperty(const FlyCapture2::PropertyType &prop_type, bool on, bool auto_on, float value, PropertyValue prop_value = ABS_VALUE); protected: diff --git a/modules/sensor/include/visp3/sensor/vpForceTorqueAtiSensor.h b/modules/sensor/include/visp3/sensor/vpForceTorqueAtiSensor.h index 364b8633a1..b3c06d65f7 100644 --- a/modules/sensor/include/visp3/sensor/vpForceTorqueAtiSensor.h +++ b/modules/sensor/include/visp3/sensor/vpForceTorqueAtiSensor.h @@ -66,8 +66,10 @@ information see http://www.comedi.org. call to the function blocks until the whole acquisition has finished. The following example shows how to get single measures from an ATI F/T -device each 10 ms (100 Hz). \code #include #include - +device each 10 ms (100 Hz). +\code +#include +#include int main(int argc, char** argv) { @@ -106,21 +108,17 @@ class VISP_EXPORT vpForceTorqueAtiSensor : public vpComedi void open(); - void setCalibrationFile(const std::string &calibfile, - unsigned short index = 1); + void setCalibrationFile(const std::string &calibfile, unsigned short index = 1); void unbias(); - friend VISP_EXPORT std::ostream & - operator<<(std::ostream &os, const vpForceTorqueAtiSensor &ati); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpForceTorqueAtiSensor &ati); protected: - std::string m_calibfile; //!< ATI calibration file FT*.cal - unsigned short m_index; //!< Index of calibration in file (default: 1) - unsigned short - m_num_axes; //!< Number of axis or gages available from the sensor - unsigned short - m_num_channels; //!< Number of channels available from the sensor - vpColVector m_sample_bias; //!< Sample value used for bias + std::string m_calibfile; //!< ATI calibration file FT*.cal + unsigned short m_index; //!< Index of calibration in file (default: 1) + unsigned short m_num_axes; //!< Number of axis or gages available from the sensor + unsigned short m_num_channels; //!< Number of channels available from the sensor + vpColVector m_sample_bias; //!< Sample value used for bias }; #endif diff --git a/modules/sensor/include/visp3/sensor/vpKinect.h b/modules/sensor/include/visp3/sensor/vpKinect.h index cd662ee505..f062e58c3c 100644 --- a/modules/sensor/include/visp3/sensor/vpKinect.h +++ b/modules/sensor/include/visp3/sensor/vpKinect.h @@ -137,22 +137,10 @@ class VISP_EXPORT vpKinect : public Freenect::FreenectDevice bool getDepthMap(vpImage &map, vpImage &Imap); bool getRGB(vpImage &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; - } + 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 &Irgb, const vpImage &Idepth, vpImage &IrgbWarped); // warp the RGB image into @@ -171,10 +159,10 @@ class VISP_EXPORT vpKinect : public Freenect::FreenectDevice 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 . + 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 diff --git a/modules/sensor/include/visp3/sensor/vpLaserScan.h b/modules/sensor/include/visp3/sensor/vpLaserScan.h index 43e496cfc6..19d63954f8 100644 --- a/modules/sensor/include/visp3/sensor/vpLaserScan.h +++ b/modules/sensor/include/visp3/sensor/vpLaserScan.h @@ -67,14 +67,14 @@ class VISP_EXPORT vpLaserScan /*! 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) + : listScanPoints(), startTimestamp(0), endTimestamp(0), measurementId(0), numSteps(0), startAngle(0), stopAngle(0), + numPoints(0) { } /*! Copy constructor. */ vpLaserScan(const vpLaserScan &scan) - : listScanPoints(scan.listScanPoints), startTimestamp(0), endTimestamp(0), - measurementId(0), numSteps(0), startAngle(0), stopAngle(0), numPoints(0) + : listScanPoints(scan.listScanPoints), startTimestamp(0), endTimestamp(0), measurementId(0), numSteps(0), + startAngle(0), stopAngle(0), numPoints(0) { startTimestamp = scan.startTimestamp; endTimestamp = scan.endTimestamp; @@ -94,40 +94,19 @@ class VISP_EXPORT vpLaserScan inline std::vector getScanPoints() { return listScanPoints; } /*! Specifies the id of former measurements and increases with every measurement. */ - inline void setMeasurementId(const unsigned short &id) - { - this->measurementId = id; - } + 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; - } + 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; - } + 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; - } + 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; - } + 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; - } + 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; - } + 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. */ diff --git a/modules/sensor/include/visp3/sensor/vpLaserScanner.h b/modules/sensor/include/visp3/sensor/vpLaserScanner.h index 1d420d6632..98f58c53db 100644 --- a/modules/sensor/include/visp3/sensor/vpLaserScanner.h +++ b/modules/sensor/include/visp3/sensor/vpLaserScanner.h @@ -61,8 +61,7 @@ class VISP_EXPORT vpLaserScanner */ vpLaserScanner() : ip("null"), port(0){}; /*! Copy constructor. */ - vpLaserScanner(const vpLaserScanner &scanner) - : ip(scanner.ip), port(scanner.port){}; + vpLaserScanner(const vpLaserScanner &scanner) : ip(scanner.ip), port(scanner.port){}; /*! Default destructor that does nothing. */ virtual ~vpLaserScanner(){}; diff --git a/modules/sensor/include/visp3/sensor/vpOpenCVGrabber.h b/modules/sensor/include/visp3/sensor/vpOpenCVGrabber.h index f5b85469e7..aa21eed13a 100644 --- a/modules/sensor/include/visp3/sensor/vpOpenCVGrabber.h +++ b/modules/sensor/include/visp3/sensor/vpOpenCVGrabber.h @@ -87,8 +87,8 @@ int main() vpOpenCVGrabber g; g.open(I); - std::cout << "Image size: " << I.getWidth() << " " << I.getHeight() << -std::endl; vpDisplayOpenCV d(I); + std::cout << "Image size: " << I.getWidth() << " " << I.getHeight() << std::endl; + vpDisplayOpenCV d(I); while(1) { g.acquire(I); diff --git a/modules/sensor/include/visp3/sensor/vpPylonFactory.h b/modules/sensor/include/visp3/sensor/vpPylonFactory.h index 175bcfc5be..382a5d7476 100644 --- a/modules/sensor/include/visp3/sensor/vpPylonFactory.h +++ b/modules/sensor/include/visp3/sensor/vpPylonFactory.h @@ -62,8 +62,7 @@ Example code. \code vpPylonFactory &factory = vpPylonFactory::instance(); - vpPylonGrabber *g = - factory.createPylonGrabber(vpPylonFactory::BASLER_GIGE); + vpPylonGrabber *g = factory.createPylonGrabber(vpPylonFactory::BASLER_GIGE); \endcode */ class VISP_EXPORT vpPylonFactory @@ -86,8 +85,7 @@ class VISP_EXPORT vpPylonFactory vpPylonFactory(vpPylonFactory const &); void operator=(vpPylonFactory const &); - Pylon::PylonAutoInitTerm - m_autoInitTerm; //!< Auto initialize and terminate object for pylon SDK. + Pylon::PylonAutoInitTerm m_autoInitTerm; //!< Auto initialize and terminate object for pylon SDK. }; #endif // #ifdef VISP_HAVE_PYLON diff --git a/modules/sensor/include/visp3/sensor/vpPylonGrabber.h b/modules/sensor/include/visp3/sensor/vpPylonGrabber.h index b5e1fa7b03..7bfb68685e 100644 --- a/modules/sensor/include/visp3/sensor/vpPylonGrabber.h +++ b/modules/sensor/include/visp3/sensor/vpPylonGrabber.h @@ -76,8 +76,7 @@ To get a vpPylonGrabber, use the following code. \code vpPylonFactory &factory = vpPylonFactory::instance(); - vpPylonGrabber *g = - factory.createPylonGrabber(vpPylonFactory::BASLER_GIGE); + vpPylonGrabber *g = factory.createPylonGrabber(vpPylonFactory::BASLER_GIGE); \endcode To use this class install first Pylon SDK\n @@ -320,8 +319,7 @@ class VISP_EXPORT vpPylonGrabber : public vpFrameGrabber https://www.ptgrey.com/kb/11020?countryid=237 \sa getExposure() */ - virtual float setExposure(bool exposure_on, bool exposure_auto, - float exposure_value) = 0; + virtual float setExposure(bool exposure_on, bool exposure_auto, float exposure_value) = 0; /*! Set camera gain mode and value. \param gain_auto : If true set auto gain, if false set manual gain @@ -367,8 +365,7 @@ class VISP_EXPORT vpPylonGrabber : public vpFrameGrabber \sa loadUserSet() */ - virtual bool saveUserSet(UserSetName user_set, - bool set_default = false) = 0; + virtual bool saveUserSet(UserSetName user_set, bool set_default = false) = 0; /*! \brief Sets the configuration set to be used as the default startup set. diff --git a/modules/sensor/include/visp3/sensor/vpRealSense.h b/modules/sensor/include/visp3/sensor/vpRealSense.h index 829f98ce9e..617e472ffc 100644 --- a/modules/sensor/include/visp3/sensor/vpRealSense.h +++ b/modules/sensor/include/visp3/sensor/vpRealSense.h @@ -98,7 +98,9 @@ target_link_libraries(sample-realsense ${VISP_LIBRARIES}) To acquire images from the RealSense color camera and convert them into grey level images, a good starting is to use the following code that corresponds to -the content of sample-realsense.cpp: \code #include +the content of sample-realsense.cpp: +\code +#include #include #include @@ -107,9 +109,12 @@ int main() vpRealSense rs; rs.open(); - vpImage I(rs.getIntrinsics(rs::stream::color).height, -rs.getIntrinsics(rs::stream::color).width); #ifdef VISP_HAVE_X11 vpDisplayX -d(I); #elif defined(VISP_HAVE_GDI) vpDisplayGDI d(I); #endif + vpImage I(rs.getIntrinsics(rs::stream::color).height, rs.getIntrinsics(rs::stream::color).width); +#ifdef VISP_HAVE_X11 + vpDisplayX d(I); +#elif defined(VISP_HAVE_GDI) + vpDisplayGDI d(I); +#endif while (1) { rs.acquire(I); @@ -124,14 +129,19 @@ d(I); #elif defined(VISP_HAVE_GDI) vpDisplayGDI d(I); #endif If you want to acquire color images, in the previous sample replace: \code - vpImage I(rs.getIntrinsics(rs::stream::color).height, -rs.getIntrinsics(rs::stream::color).width); \endcode by \code vpImage -I(rs.getIntrinsics(rs::stream::color).height, -rs.getIntrinsics(rs::stream::color).width); \endcode + vpImage I(rs.getIntrinsics(rs::stream::color).height, rs.getIntrinsics(rs::stream::color).width); + \endcode + by + \code + vpImage I(rs.getIntrinsics(rs::stream::color).height, rs.getIntrinsics(rs::stream::color).width); + \endcode If you are interested in the point cloud and if ViSP is build with PCL support, you can start from the following example where we use PCL library to -visualize the point cloud: \code #include +visualize the point cloud: + +\code +#include #include #include @@ -142,15 +152,13 @@ int main() rs.open(); std::cout << rs << std::endl; - pcl::PointCloud::Ptr pointcloud(new -pcl::PointCloud); + pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud); rs.acquire(pointcloud); - pcl::visualization::PCLVisualizer::Ptr viewer (new -pcl::visualization::PCLVisualizer ("3D Viewer")); - pcl::visualization::PointCloudColorHandlerRGBField -rgb(pointcloud); viewer->setBackgroundColor (0, 0, 0); + pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); + pcl::visualization::PointCloudColorHandlerRGBField rgb(pointcloud); + viewer->setBackgroundColor (0, 0, 0); viewer->addCoordinateSystem (1.0); viewer->initCameraParameters (); viewer->setCameraPosition(0,0,-0.5, 0,-1,0); @@ -160,14 +168,12 @@ rgb(pointcloud); viewer->setBackgroundColor (0, 0, 0); static bool update = false; if (! update) { - viewer->addPointCloud (pointcloud, rgb, "sample -cloud"); viewer->setPointCloudRenderingProperties -(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); update = -true; + viewer->addPointCloud (pointcloud, rgb, "sample cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); + update = true; } else { - viewer->updatePointCloud (pointcloud, rgb, "sample -cloud"); + viewer->updatePointCloud (pointcloud, rgb, "sample cloud"); } viewer->spinOnce (100); @@ -179,9 +185,12 @@ cloud"); If you want to change the default stream parameters, you can use \p setEnableStream() to enable only the desired stream and \p setStreamSettings() to set the stream settings. The following code allows to capture the color -stream in 1920x1080 also with the infrared stream: \code #include - #include #include - +stream in 1920x1080 also with the infrared stream: + +\code +#include +#include +#include int main() { vpRealSense rs; @@ -189,14 +198,12 @@ int main() { rs.setEnableStream(rs::stream::depth, false); rs.setEnableStream(rs::stream::infrared, true); rs.setEnableStream(rs::stream::infrared2, false); - rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(1920, -1080, rs::format::rgba8, 30)); rs.setStreamSettings(rs::stream::infrared, -vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 30)); rs.open(); + rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(1920, 1080, rs::format::rgba8, 30)); + rs.setStreamSettings(rs::stream::infrared, vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 30)); + rs.open(); - vpImage Ic(rs.getIntrinsics(rs::stream::color).height, -rs.getIntrinsics(rs::stream::color).width); vpImage -Ii(rs.getIntrinsics(rs::stream::infrared).height, -rs.getIntrinsics(rs::stream::infrared).width); + vpImage Ic(rs.getIntrinsics(rs::stream::color).height, rs.getIntrinsics(rs::stream::color).width); + vpImage Ii(rs.getIntrinsics(rs::stream::infrared).height, rs.getIntrinsics(rs::stream::infrared).width); #ifdef VISP_HAVE_X11 vpDisplayX dc(Ic, 0, 0, "Color"); @@ -232,16 +239,13 @@ int main() { rs.setEnableStream(rs::stream::depth, true); rs.setEnableStream(rs::stream::infrared, false); rs.setEnableStream(rs::stream::infrared2, false); - rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, -480, rs::format::rgba8, 30)); rs.setStreamSettings(rs::stream::depth, -vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 30)); rs.open(); + rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30)); + rs.setStreamSettings(rs::stream::depth, vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 30)); + rs.open(); - vpImage Ic(rs.getIntrinsics(rs::stream::color).height, -rs.getIntrinsics(rs::stream::color).width); vpImage -Id_raw(rs.getIntrinsics(rs::stream::depth).height, -rs.getIntrinsics(rs::stream::depth).width); vpImage -Id(rs.getIntrinsics(rs::stream::depth).height, -rs.getIntrinsics(rs::stream::depth).width); + vpImage Ic(rs.getIntrinsics(rs::stream::color).height, rs.getIntrinsics(rs::stream::color).width); + vpImage Id_raw(rs.getIntrinsics(rs::stream::depth).height, rs.getIntrinsics(rs::stream::depth).width); + vpImage Id(rs.getIntrinsics(rs::stream::depth).height, rs.getIntrinsics(rs::stream::depth).width); #ifdef VISP_HAVE_X11 vpDisplayX dc(Ic, 0, 0, "Color"); @@ -252,8 +256,8 @@ rs.getIntrinsics(rs::stream::depth).width); #endif while (1) { - rs.acquire((unsigned char *) Ic.bitmap, (unsigned char *) Id_raw.bitmap, -NULL, NULL, NULL, rs::stream::color, rs::stream::depth_aligned_to_color); + rs.acquire((unsigned char *) Ic.bitmap, (unsigned char *) Id_raw.bitmap, NULL, NULL, NULL, + rs::stream::color, rs::stream::depth_aligned_to_color); vpImageConvert::createDepthHistogram(Id_raw, Id); vpDisplay::display(Ic); vpDisplay::display(Id); @@ -267,7 +271,10 @@ NULL, NULL, NULL, rs::stream::color, rs::stream::depth_aligned_to_color); \endcode This is how you get intrinsics for non native stream (the native stream has -to be enabled!): \code #include + to be enabled!): + +\code +#include int main() { vpRealSense rs; @@ -275,17 +282,14 @@ int main() { rs.setEnableStream(rs::stream::depth, true); rs.setEnableStream(rs::stream::infrared, false); rs.setEnableStream(rs::stream::infrared2, false); - rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, -480, rs::format::rgba8, 30)); rs.setStreamSettings(rs::stream::depth, -vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 30)); rs.open(); + rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30)); + rs.setStreamSettings(rs::stream::depth, vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 30)); + rs.open(); rs::device * dev = rs.getHandler(); - rs::intrinsics depth_aligned_intrinsic = -dev->get_stream_intrinsics(rs::stream::depth_aligned_to_color); std::cout << -"Intrinsics [fx, fy, ppx, ppy]: " << depth_aligned_intrinsic.fx << " ; " << -depth_aligned_intrinsic.fy - << " ; " << depth_aligned_intrinsic.ppx << " ; " << -depth_aligned_intrinsic.ppy << std::endl; return 0; + rs::intrinsics depth_aligned_intrinsic = dev->get_stream_intrinsics(rs::stream::depth_aligned_to_color); + std::cout << "Intrinsics [fx, fy, ppx, ppy]: " << depth_aligned_intrinsic.fx << " ; " << depth_aligned_intrinsic.fy + << " ; " << depth_aligned_intrinsic.ppx << " ; " << depth_aligned_intrinsic.ppy << std::endl; return 0; } \endcode @@ -299,17 +303,14 @@ int main() { rs.open(); rs::device *device = rs.getHandler(); - std::cout << "Stream width: " << device->get_stream_width(rs::stream::color) -<< std::endl; std::cout << "Stream height: " << -device->get_stream_height(rs::stream::color) << std::endl; std::cout << -"Stream format: " << device->get_stream_format(rs::stream::color) << -std::endl; std::cout << "Stream framerate: " << -device->get_stream_framerate(rs::stream::color) << std::endl; + std::cout << "Stream width: " << device->get_stream_width(rs::stream::color) << std::endl; + std::cout << "Stream height: " << device->get_stream_height(rs::stream::color) << std::endl; + std::cout << "Stream format: " << device->get_stream_format(rs::stream::color) << std::endl; + std::cout << "Stream framerate: " << device->get_stream_framerate(rs::stream::color) << std::endl; std::cout << "API version: " << rs_get_api_version(nullptr) << std::endl; - std::cout << "Firmware: " << rs_get_device_firmware_version((const rs_device -*) device, nullptr) << std::endl; std::cout << "RealSense sensor -characteristics: \n" << rs << std::endl; + std::cout << "Firmware: " << rs_get_device_firmware_version((const rs_device *) device, nullptr) << std::endl; + std::cout << "RealSense sensor characteristics: \n" << rs << std::endl; return 0; } @@ -318,8 +319,8 @@ characteristics: \n" << rs << std::endl; Camera parameters can be set in the following manner: \code rs::device * dev = rs.getHandler(); - dev->set_option(rs::option::r200_lr_auto_exposure_enabled, 1); //enable lr -auto exposure for the R200 \endcode + dev->set_option(rs::option::r200_lr_auto_exposure_enabled, 1); //enable lr auto exposure for the R200 + \endcode \note This class has been tested with the Intel RealSense SR300 (Firmware: 3.15.0.0) and R200 (Firmware: 1.0.71.06) using librealsense (API @@ -341,66 +342,46 @@ class VISP_EXPORT vpRealSense void acquire(pcl::PointCloud::Ptr &pointcloud); #endif void acquire(vpImage &grey); // tested - void acquire(vpImage &grey, - std::vector &pointcloud); - void acquire(vpImage &grey, vpImage &infrared, - vpImage &depth, + void acquire(vpImage &grey, std::vector &pointcloud); + void acquire(vpImage &grey, vpImage &infrared, vpImage &depth, std::vector &pointcloud); #ifdef VISP_HAVE_PCL - void acquire(vpImage &grey, - pcl::PointCloud::Ptr &pointcloud); - void acquire(vpImage &grey, vpImage &infrared, - vpImage &depth, + void acquire(vpImage &grey, pcl::PointCloud::Ptr &pointcloud); + void acquire(vpImage &grey, vpImage &infrared, vpImage &depth, pcl::PointCloud::Ptr &pointcloud); - void acquire(vpImage &grey, vpImage &infrared, - vpImage &depth, + void acquire(vpImage &grey, vpImage &infrared, vpImage &depth, pcl::PointCloud::Ptr &pointcloud); #endif void acquire(vpImage &color); // tested void acquire(vpImage &color, std::vector &pointcloud); - void acquire(vpImage &color, vpImage &infrared, - vpImage &depth, + void acquire(vpImage &color, vpImage &infrared, vpImage &depth, std::vector &pointcloud); - void acquire(unsigned char *const data_image, - unsigned char *const data_depth, - std::vector *const data_pointCloud, - unsigned char *const data_infrared, - unsigned char *const data_infrared2 = NULL, - const rs::stream &stream_color = rs::stream::color, + void acquire(unsigned char *const data_image, unsigned char *const data_depth, + std::vector *const data_pointCloud, unsigned char *const data_infrared, + unsigned char *const data_infrared2 = NULL, const rs::stream &stream_color = rs::stream::color, const rs::stream &stream_depth = rs::stream::depth, const rs::stream &stream_infrared = rs::stream::infrared, const rs::stream &stream_infrared2 = rs::stream::infrared2); #ifdef VISP_HAVE_PCL - void acquire(vpImage &color, - pcl::PointCloud::Ptr &pointcloud); - void acquire(vpImage &color, vpImage &infrared, - vpImage &depth, + void acquire(vpImage &color, pcl::PointCloud::Ptr &pointcloud); + void acquire(vpImage &color, vpImage &infrared, vpImage &depth, pcl::PointCloud::Ptr &pointcloud); - void acquire(vpImage &color, vpImage &infrared, - vpImage &depth, + void acquire(vpImage &color, vpImage &infrared, vpImage &depth, pcl::PointCloud::Ptr &pointcloud); - void acquire(unsigned char *const data_image, - unsigned char *const data_depth, - std::vector *const data_pointCloud, - pcl::PointCloud::Ptr &pointcloud, - unsigned char *const data_infrared, - unsigned char *const data_infrared2 = NULL, - const rs::stream &stream_color = rs::stream::color, - const rs::stream &stream_depth = rs::stream::depth, + void acquire(unsigned char *const data_image, unsigned char *const data_depth, + std::vector *const data_pointCloud, pcl::PointCloud::Ptr &pointcloud, + unsigned char *const data_infrared, unsigned char *const data_infrared2 = NULL, + const rs::stream &stream_color = rs::stream::color, const rs::stream &stream_depth = rs::stream::depth, const rs::stream &stream_infrared = rs::stream::infrared, const rs::stream &stream_infrared2 = rs::stream::infrared2); - void acquire(unsigned char *const data_image, - unsigned char *const data_depth, - std::vector *const data_pointCloud, - pcl::PointCloud::Ptr &pointcloud, - unsigned char *const data_infrared, - unsigned char *const data_infrared2 = NULL, - const rs::stream &stream_color = rs::stream::color, - const rs::stream &stream_depth = rs::stream::depth, + void acquire(unsigned char *const data_image, unsigned char *const data_depth, + std::vector *const data_pointCloud, pcl::PointCloud::Ptr &pointcloud, + unsigned char *const data_infrared, unsigned char *const data_infrared2 = NULL, + const rs::stream &stream_color = rs::stream::color, const rs::stream &stream_depth = rs::stream::depth, const rs::stream &stream_infrared = rs::stream::infrared, const rs::stream &stream_infrared2 = rs::stream::infrared2); #endif @@ -409,13 +390,11 @@ class VISP_EXPORT vpRealSense vpCameraParameters getCameraParameters( const rs::stream &stream, - vpCameraParameters::vpCameraParametersProjType type = - vpCameraParameters::perspectiveProjWithDistortion) const; + vpCameraParameters::vpCameraParametersProjType type = vpCameraParameters::perspectiveProjWithDistortion) const; //! Get access to device handler rs::device *getHandler() const { return m_device; } - rs::extrinsics getExtrinsics(const rs::stream &from, - const rs::stream &to) const; + rs::extrinsics getExtrinsics(const rs::stream &from, const rs::stream &to) const; rs::intrinsics getIntrinsics(const rs::stream &stream) const; //! Get the value used when the pixel value (u, v) in the depth map is @@ -428,15 +407,13 @@ class VISP_EXPORT vpRealSense //! Get device serial number. //! \sa setDeviceBySerialNumber() std::string getSerialNumber() const { return m_serial_no; } - vpHomogeneousMatrix getTransformation(const rs::stream &from, - const rs::stream &to) const; + vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) const; void open(); void setDeviceBySerialNumber(const std::string &serial_no); - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpRealSense &rs); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpRealSense &rs); struct vpRsStreamParams { int m_streamWidth; @@ -445,16 +422,14 @@ class VISP_EXPORT vpRealSense int m_streamFramerate; vpRsStreamParams() - : m_streamWidth(640), m_streamHeight(480), - m_streamFormat(rs::format::rgba8), m_streamFramerate(30) + : m_streamWidth(640), m_streamHeight(480), m_streamFormat(rs::format::rgba8), m_streamFramerate(30) { } - vpRsStreamParams(const int streamWidth, const int streamHeight, - const rs::format &streamFormat, + vpRsStreamParams(const int streamWidth, const int streamHeight, const rs::format &streamFormat, const int streamFramerate) - : m_streamWidth(streamWidth), m_streamHeight(streamHeight), - m_streamFormat(streamFormat), m_streamFramerate(streamFramerate) + : m_streamWidth(streamWidth), m_streamHeight(streamHeight), m_streamFormat(streamFormat), + m_streamFramerate(streamFramerate) { } }; @@ -464,14 +439,10 @@ class VISP_EXPORT vpRealSense //! Set the value used when the pixel value (u, v) in the depth map is //! invalid for the point cloud. For instance, the Point Cloud Library (PCL) //! uses NAN values for points where the depth is invalid. - inline void setInvalidDepthValue(const float value) - { - m_invalidDepthValue = value; - } + inline void setInvalidDepthValue(const float value) { m_invalidDepthValue = value; } void setStreamSettings(const rs::stream &stream, const rs::preset &preset); - void setStreamSettings(const rs::stream &stream, - const vpRsStreamParams ¶ms); + void setStreamSettings(const rs::stream &stream, const vpRsStreamParams ¶ms); protected: rs::context m_context; diff --git a/modules/sensor/include/visp3/sensor/vpRealSense2.h b/modules/sensor/include/visp3/sensor/vpRealSense2.h index 26295b6ea7..08ed117df5 100644 --- a/modules/sensor/include/visp3/sensor/vpRealSense2.h +++ b/modules/sensor/include/visp3/sensor/vpRealSense2.h @@ -119,15 +119,18 @@ d(I); #elif defined(VISP_HAVE_GDI) vpDisplayGDI d(I); #endif If you want to acquire color images, in the previous sample replace: \code - vpImage I(rs.getIntrinsics(RS2_STREAM_COLOR).height, -rs.getIntrinsics(RS2_STREAM_COLOR).width); \endcode by \code vpImage -I(rs.getIntrinsics(RS2_STREAM_COLOR).height, -rs.getIntrinsics(RS2_STREAM_COLOR).width); \endcode + vpImage I(rs.getIntrinsics(RS2_STREAM_COLOR).height, rs.getIntrinsics(RS2_STREAM_COLOR).width); + \endcode + by + \code + vpImage I(rs.getIntrinsics(RS2_STREAM_COLOR).height, rs.getIntrinsics(RS2_STREAM_COLOR).width); + \endcode If you are interested in the point cloud and if ViSP is build with PCL -support, you can start from the following example where we use PCL library to -visualize the point cloud: \code #include - + support, you can start from the following example where we use PCL library to + visualize the point cloud: + \code +#include #include #include @@ -136,15 +139,13 @@ int main() vpRealSense2 rs; rs.open(); - pcl::PointCloud::Ptr pointcloud(new -pcl::PointCloud); + pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud); rs.acquire(NULL, NULL, NULL, pointcloud); - pcl::visualization::PCLVisualizer::Ptr viewer (new -pcl::visualization::PCLVisualizer ("3D Viewer")); - pcl::visualization::PointCloudColorHandlerRGBField -rgb(pointcloud); viewer->setBackgroundColor(0, 0, 0); + pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); + pcl::visualization::PointCloudColorHandlerRGBField rgb(pointcloud); + viewer->setBackgroundColor(0, 0, 0); viewer->initCameraParameters(); viewer->setCameraPosition(0, 0, -0.5, 0, -1, 0); @@ -153,11 +154,11 @@ rgb(pointcloud); viewer->setBackgroundColor(0, 0, 0); static bool update = false; if (!update) { - viewer->addPointCloud (pointcloud, rgb, "sample -cloud"); viewer->setPointCloudRenderingProperties -(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); update = -true; } else { viewer->updatePointCloud (pointcloud, rgb, -"sample cloud"); + viewer->addPointCloud (pointcloud, rgb, "sample cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); + update = true; + } else { + viewer->updatePointCloud (pointcloud, rgb, "sample cloud"); } viewer->spinOnce(30); @@ -168,9 +169,11 @@ true; } else { viewer->updatePointCloud (pointcloud, rgb, If you want to change the default stream parameters, refer to the librealsense2 `rs2::config` documentation. The following code allows to -capture the color stream in 1920x1080: \code #include - #include #include - +capture the color stream in 1920x1080: +\code +#include +#include +#include int main() { vpRealSense2 rs; @@ -180,10 +183,9 @@ int main() { config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30); rs.open(config); - vpImage Ic(rs.getIntrinsics(RS2_STREAM_COLOR).height, -rs.getIntrinsics(RS2_STREAM_COLOR).width); vpImage -Ii(rs.getIntrinsics(RS2_STREAM_INFRARED).height, -rs.getIntrinsics(RS2_STREAM_INFRARED).width); + vpImage Ic(rs.getIntrinsics(RS2_STREAM_COLOR).height, rs.getIntrinsics(RS2_STREAM_COLOR).width); + vpImage Ii(rs.getIntrinsics(RS2_STREAM_INFRARED).height, + rs.getIntrinsics(RS2_STREAM_INFRARED).width); #ifdef VISP_HAVE_X11 vpDisplayX dc(Ic, 0, 0, "Color"); @@ -221,12 +223,9 @@ int main() { config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30); rs.open(config); - vpImage Ic(rs.getIntrinsics(RS2_STREAM_COLOR).height, -rs.getIntrinsics(RS2_STREAM_COLOR).width); vpImage -Id_raw(rs.getIntrinsics(RS2_STREAM_DEPTH).height, -rs.getIntrinsics(RS2_STREAM_DEPTH).width); vpImage -Id(rs.getIntrinsics(RS2_STREAM_DEPTH).height, -rs.getIntrinsics(RS2_STREAM_DEPTH).width); + vpImage Ic(rs.getIntrinsics(RS2_STREAM_COLOR).height, rs.getIntrinsics(RS2_STREAM_COLOR).width); + vpImage Id_raw(rs.getIntrinsics(RS2_STREAM_DEPTH).height, rs.getIntrinsics(RS2_STREAM_DEPTH).width); + vpImage Id(rs.getIntrinsics(RS2_STREAM_DEPTH).height, rs.getIntrinsics(RS2_STREAM_DEPTH).width); #ifdef VISP_HAVE_X11 vpDisplayX dc(Ic, 0, 0, "Color"); @@ -238,8 +237,8 @@ rs.getIntrinsics(RS2_STREAM_DEPTH).width); rs2::align align_to(RS2_STREAM_COLOR); while (true) { - rs.acquire((unsigned char *) Ic.bitmap, (unsigned char *) Id_raw.bitmap, -NULL, NULL, &align_to); vpImageConvert::createDepthHistogram(Id_raw, Id); + rs.acquire((unsigned char *) Ic.bitmap, (unsigned char *) Id_raw.bitmap, NULL, NULL, &align_to); + vpImageConvert::createDepthHistogram(Id_raw, Id); vpDisplay::display(Ic); vpDisplay::display(Id); vpDisplay::flush(Ic); @@ -252,8 +251,9 @@ NULL, NULL, &align_to); vpImageConvert::createDepthHistogram(Id_raw, Id); \endcode References to `rs2::pipeline_profile` and `rs2::pipeline` can be retrieved -with (`rs.open() must be called before`): \code rs2::pipeline_profile& profile -= rs.getPipelineProfile(); rs2::pipeline& pipeline = rs.getPipeline(); + with (`rs.open() must be called before`): + \code +rs2::pipeline_profile& profile = rs.getPipelineProfile(); rs2::pipeline& pipeline = rs.getPipeline(); \endcode Information about the sensor can be printed with: @@ -283,33 +283,24 @@ class VISP_EXPORT vpRealSense2 void acquire(vpImage &grey); void acquire(vpImage &color); - void acquire(unsigned char *const data_image, - unsigned char *const data_depth, - std::vector *const data_pointCloud, - unsigned char *const data_infrared, + void acquire(unsigned char *const data_image, unsigned char *const data_depth, + std::vector *const data_pointCloud, unsigned char *const data_infrared, rs2::align *const align_to = NULL); #ifdef VISP_HAVE_PCL - void acquire(unsigned char *const data_image, - unsigned char *const data_depth, - std::vector *const data_pointCloud, - pcl::PointCloud::Ptr &pointcloud, - unsigned char *const data_infrared = NULL, - rs2::align *const align_to = NULL); - void acquire(unsigned char *const data_image, - unsigned char *const data_depth, - std::vector *const data_pointCloud, - pcl::PointCloud::Ptr &pointcloud, - unsigned char *const data_infrared = NULL, - rs2::align *const align_to = NULL); + void acquire(unsigned char *const data_image, unsigned char *const data_depth, + std::vector *const data_pointCloud, pcl::PointCloud::Ptr &pointcloud, + unsigned char *const data_infrared = NULL, rs2::align *const align_to = NULL); + void acquire(unsigned char *const data_image, unsigned char *const data_depth, + std::vector *const data_pointCloud, pcl::PointCloud::Ptr &pointcloud, + unsigned char *const data_infrared = NULL, rs2::align *const align_to = NULL); #endif void close(); vpCameraParameters getCameraParameters( const rs2_stream &stream, - vpCameraParameters::vpCameraParametersProjType type = - vpCameraParameters::perspectiveProjWithDistortion) const; + vpCameraParameters::vpCameraParametersProjType type = vpCameraParameters::perspectiveProjWithDistortion) const; rs2_intrinsics getIntrinsics(const rs2_stream &stream) const; @@ -328,21 +319,16 @@ class VISP_EXPORT vpRealSense2 //! Get a reference to `rs2::pipeline_profile`. rs2::pipeline_profile &getPipelineProfile() { return m_pipelineProfile; } - vpHomogeneousMatrix getTransformation(const rs2_stream &from, - const rs2_stream &to) const; + vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to) const; void open(const rs2::config &cfg = rs2::config()); - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpRealSense2 &rs); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpRealSense2 &rs); //! Set the value used when the pixel value (u, v) in the depth map is //! invalid for the point cloud. For instance, the Point Cloud Library (PCL) //! uses NAN values for points where the depth is invalid. - inline void setInvalidDepthValue(const float value) - { - m_invalidDepthValue = value; - } + inline void setInvalidDepthValue(const float value) { m_invalidDepthValue = value; } //! Set the maximum Z value (used to discard bad reconstructed depth for //! pointcloud). @@ -363,13 +349,10 @@ class VISP_EXPORT vpRealSense2 void getColorFrame(const rs2::frame &frame, vpImage &color); void getGreyFrame(const rs2::frame &frame, vpImage &grey); void getNativeFrameData(const rs2::frame &frame, unsigned char *const data); - void getPointcloud(const rs2::depth_frame &depth_frame, - std::vector &pointcloud); + void getPointcloud(const rs2::depth_frame &depth_frame, std::vector &pointcloud); #ifdef VISP_HAVE_PCL - void getPointcloud(const rs2::depth_frame &depth_frame, - pcl::PointCloud::Ptr &pointcloud); - void getPointcloud(const rs2::depth_frame &depth_frame, - const rs2::frame &color_frame, + void getPointcloud(const rs2::depth_frame &depth_frame, pcl::PointCloud::Ptr &pointcloud); + void getPointcloud(const rs2::depth_frame &depth_frame, const rs2::frame &color_frame, pcl::PointCloud::Ptr &pointcloud); #endif }; diff --git a/modules/sensor/include/visp3/sensor/vpScanPoint.h b/modules/sensor/include/visp3/sensor/vpScanPoint.h index c797f46b5f..a73ed61b76 100644 --- a/modules/sensor/include/visp3/sensor/vpScanPoint.h +++ b/modules/sensor/include/visp3/sensor/vpScanPoint.h @@ -78,8 +78,7 @@ class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not /*! Default constructor. */ inline vpScanPoint() : rDist(0), hAngle(0), vAngle(0) {} /*! Copy constructor. */ - inline vpScanPoint(const vpScanPoint &scanpoint) - : rDist(0), hAngle(0), vAngle(0) + inline vpScanPoint(const vpScanPoint &scanpoint) : rDist(0), hAngle(0), vAngle(0) { this->rDist = scanpoint.rDist; this->hAngle = scanpoint.hAngle; @@ -91,8 +90,7 @@ class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not \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) + 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; @@ -131,10 +129,7 @@ class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not positive in front of the laser while y on the left side. */ - inline double getX() const - { - return (rDist * cos(this->hAngle) * cos(this->vAngle)); - } + inline double getX() const { return (rDist * cos(this->hAngle) * cos(this->vAngle)); } /*! Returns the cartesian y coordinate. @@ -149,21 +144,16 @@ class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not The z axis is vertical and oriented in direction of the sky. */ - inline double getZ() const - { - return (rDist * cos(this->hAngle) * sin(this->vAngle)); - } + inline double getZ() const { return (rDist * cos(this->hAngle) * sin(this->vAngle)); } - friend inline std::ostream &operator<<(std::ostream &s, - const vpScanPoint &p); + 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) + friend inline bool operator==(const vpScanPoint &sp1, const vpScanPoint &sp2) { // return ( ( sp1.getRadialDist() == sp2.getRadialDist() ) // && ( sp1.getHAngle() == sp2.getHAngle() ) @@ -175,15 +165,9 @@ class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not double ha2 = sp2.getHAngle(); double va2 = sp2.getVAngle(); - return ( - (std::fabs(rd1 - rd2) <= - std::fabs(vpMath::maximum(rd1, rd2)) * - std::numeric_limits::epsilon()) && - (std::fabs(ha1 - ha2) <= - std::fabs(vpMath::maximum(ha1, ha2)) * - std::numeric_limits::epsilon()) && - (std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1, va2)) * - std::numeric_limits::epsilon())); + return ((std::fabs(rd1 - rd2) <= std::fabs(vpMath::maximum(rd1, rd2)) * std::numeric_limits::epsilon()) && + (std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1, ha2)) * std::numeric_limits::epsilon()) && + (std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1, va2)) * std::numeric_limits::epsilon())); } /*! @@ -191,8 +175,7 @@ class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not Returns true if sp1 and sp2 are different; otherwire returns false. */ - friend inline bool operator!=(const vpScanPoint &sp1, - const vpScanPoint &sp2) + friend inline bool operator!=(const vpScanPoint &sp1, const vpScanPoint &sp2) { // return ( ( sp1.getRadialDist() != sp2.getRadialDist() ) // || ( sp1.getHAngle() != sp2.getHAngle() ) @@ -203,14 +186,9 @@ class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not 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::epsilon()) || - (std::fabs(ha1 - ha2) <= - std::fabs(vpMath::maximum(ha1, ha2)) * - std::numeric_limits::epsilon()) || - (std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1, va2)) * - std::numeric_limits::epsilon())); + return ((std::fabs(rd1 - rd2) > std::fabs(vpMath::maximum(rd1, rd2)) * std::numeric_limits::epsilon()) || + (std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1, ha2)) * std::numeric_limits::epsilon()) || + (std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1, va2)) * std::numeric_limits::epsilon())); } private: @@ -240,14 +218,14 @@ class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not 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 + 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); + p.setPolar(radialDistance, horizontalAngle, verticalAngle); -std::cout << p << std::endl; + std::cout << p << std::endl; } \endcode will produce the prints @@ -261,8 +239,8 @@ 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 << p.getRadialDist() << " " << p.getHAngle() << " " << p.getVAngle() << " " << p.getX() << " " << p.getY() << " " + << p.getZ(); s.setf(original_flags); // restore s to standard state diff --git a/modules/sensor/include/visp3/sensor/vpSickLDMRS.h b/modules/sensor/include/visp3/sensor/vpSickLDMRS.h index 69d408f225..1cdc7fc068 100644 --- a/modules/sensor/include/visp3/sensor/vpSickLDMRS.h +++ b/modules/sensor/include/visp3/sensor/vpSickLDMRS.h @@ -40,8 +40,7 @@ #include -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) #include #include @@ -94,8 +93,7 @@ int main() // Prints all the measured points for (int layer=0; layer<4; layer++) { - std::vector pointsInLayer = -laserscan[layer].getScanPoints(); vpScanPoint p; + std::vector pointsInLayer = laserscan[layer].getScanPoints(); vpScanPoint p; for (unsigned int i=0; i < pointsInLayer.size(); i++) { std::cout << pointsInLayer[i] << std::endl; @@ -120,8 +118,8 @@ class VISP_EXPORT vpSickLDMRS : public vpLaserScanner vpSickLDMRS(); /*! Copy constructor. */ vpSickLDMRS(const vpSickLDMRS &sick) - : vpLaserScanner(sick), socket_fd(-1), body(NULL), vAngle(), - time_offset(0), isFirstMeasure(true), maxlen_body(104000) + : vpLaserScanner(sick), socket_fd(-1), body(NULL), vAngle(), time_offset(0), isFirstMeasure(true), + maxlen_body(104000) { *this = sick; }; diff --git a/modules/sensor/include/visp3/sensor/vpV4l2Grabber.h b/modules/sensor/include/visp3/sensor/vpV4l2Grabber.h index a85301bd16..ad1c62b0bc 100644 --- a/modules/sensor/include/visp3/sensor/vpV4l2Grabber.h +++ b/modules/sensor/include/visp3/sensor/vpV4l2Grabber.h @@ -214,12 +214,9 @@ class VISP_EXPORT vpV4l2Grabber : public vpFrameGrabber public: vpV4l2Grabber(); explicit vpV4l2Grabber(bool verbose); - vpV4l2Grabber(unsigned input, - unsigned scale = vpV4l2Grabber::DEFAULT_SCALE); - vpV4l2Grabber(vpImage &I, unsigned input, - unsigned scale = vpV4l2Grabber::DEFAULT_SCALE); - vpV4l2Grabber(vpImage &I, unsigned input, - unsigned scale = vpV4l2Grabber::DEFAULT_SCALE); + vpV4l2Grabber(unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE); + vpV4l2Grabber(vpImage &I, unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE); + vpV4l2Grabber(vpImage &I, unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE); virtual ~vpV4l2Grabber(); public: @@ -228,12 +225,10 @@ class VISP_EXPORT vpV4l2Grabber : public vpFrameGrabber void acquire(vpImage &I); void acquire(vpImage &I, const vpRect &roi); - void acquire(vpImage &I, struct timeval ×tamp, - const vpRect &roi = vpRect()); + void acquire(vpImage &I, struct timeval ×tamp, const vpRect &roi = vpRect()); void acquire(vpImage &I); void acquire(vpImage &I, const vpRect &roi); - void acquire(vpImage &I, struct timeval ×tamp, - const vpRect &roi = vpRect()); + void acquire(vpImage &I, struct timeval ×tamp, const vpRect &roi = vpRect()); bool getField(); vpV4l2FramerateType getFramerate(); /*! @@ -243,10 +238,7 @@ class VISP_EXPORT vpV4l2Grabber : public vpFrameGrabber \return Camera pixel format coding. */ - inline vpV4l2PixelFormatType getPixelFormat() - { - return (this->m_pixelformat); - } + inline vpV4l2PixelFormatType getPixelFormat() { return (this->m_pixelformat); } vpV4l2Grabber &operator>>(vpImage &I); vpV4l2Grabber &operator>>(vpImage &I); @@ -292,10 +284,7 @@ class VISP_EXPORT vpV4l2Grabber : public vpFrameGrabber \param devname : Device name (like /dev/video0). */ - inline void setDevice(const std::string &devname) - { - sprintf(device, "%s", devname.c_str()); - } + 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 @@ -322,10 +311,7 @@ class VISP_EXPORT vpV4l2Grabber : public vpFrameGrabber - 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; - } + inline void setFrameFormat(vpV4l2FrameFormatType frameformat) { this->m_frameformat = frameformat; } void open(); void getCapabilities(); void startStreaming(); diff --git a/modules/sensor/src/force-torque/vpComedi.cpp b/modules/sensor/src/force-torque/vpComedi.cpp index d50f22cb32..c9a2c85168 100644 --- a/modules/sensor/src/force-torque/vpComedi.cpp +++ b/modules/sensor/src/force-torque/vpComedi.cpp @@ -49,9 +49,8 @@ Default constructor. */ vpComedi::vpComedi() - : m_device("/dev/comedi0"), m_handler(NULL), m_subdevice(0), m_range(0), - m_aref(AREF_DIFF), m_nchannel(6), m_range_info(6), m_maxdata(6), - m_chanlist(6) + : m_device("/dev/comedi0"), m_handler(NULL), m_subdevice(0), m_range(0), m_aref(AREF_DIFF), m_nchannel(6), + m_range_info(6), m_maxdata(6), m_chanlist(6) { } @@ -70,8 +69,7 @@ void vpComedi::open() m_handler = comedi_open(m_device.c_str()); if (!m_handler) { - throw vpException(vpException::fatalError, "Could not open device %s", - m_device.c_str()); + throw vpException(vpException::fatalError, "Could not open device %s", m_device.c_str()); } // Print NaN for clipped inputs @@ -83,10 +81,8 @@ void vpComedi::open() m_chanlist.resize(m_nchannel); for (unsigned int channel = 0; channel < m_nchannel; channel++) { m_chanlist[channel] = CR_PACK(channel, m_range, m_aref); - m_range_info[channel] = - comedi_get_range(m_handler, m_subdevice, channel, m_range); - m_maxdata[channel] = - comedi_get_maxdata(m_handler, m_subdevice, channel); + m_range_info[channel] = comedi_get_range(m_handler, m_subdevice, channel, m_range); + m_maxdata[channel] = comedi_get_maxdata(m_handler, m_subdevice, channel); } } } @@ -121,15 +117,12 @@ std::vector vpComedi::getRawData() const // When switching the multiplexor from one channel to the next, the A/D // input needs time to settle to the new input voltage. The greater the // voltage difference, the more time it takes. Here we wait for 1us - int ret = - comedi_data_read_delayed(m_handler, m_subdevice, channel, m_range, - m_aref, &raw_data[channel], 1000); + int ret = comedi_data_read_delayed(m_handler, m_subdevice, channel, m_range, m_aref, &raw_data[channel], 1000); if (ret < 0) { throw vpException(vpException::fatalError, "Cannot get %d data from device=%s subdevice=%d " "channel=%d range=%d analog reference=%d", - m_nchannel, m_device.c_str(), m_subdevice, channel, - m_aref); + m_nchannel, m_device.c_str(), m_subdevice, channel, m_aref); } } @@ -151,12 +144,9 @@ vpColVector vpComedi::getPhyData() const // Convert data to physical data for (unsigned int channel = 0; channel < m_nchannel; channel++) { - phy_data[channel] = comedi_to_phys( - raw_data[channel], m_range_info[channel], m_maxdata[channel]); + phy_data[channel] = comedi_to_phys(raw_data[channel], m_range_info[channel], m_maxdata[channel]); if (vpMath::isNaN(phy_data[channel])) { - throw vpException( - vpException::fatalError, - "Comedi DAQ get NaN value. Check the connection with your device"); + throw vpException(vpException::fatalError, "Comedi DAQ get NaN value. Check the connection with your device"); } } diff --git a/modules/sensor/src/force-torque/vpForceTorqueAtiSensor.cpp b/modules/sensor/src/force-torque/vpForceTorqueAtiSensor.cpp index abdf4fa418..722322be8c 100644 --- a/modules/sensor/src/force-torque/vpForceTorqueAtiSensor.cpp +++ b/modules/sensor/src/force-torque/vpForceTorqueAtiSensor.cpp @@ -45,15 +45,13 @@ #include #include -static Calibration *s_calibinfo = - NULL; //!< Struct containing calibration information +static Calibration *s_calibinfo = NULL; //!< Struct containing calibration information /*! * Default constructor. */ vpForceTorqueAtiSensor::vpForceTorqueAtiSensor() - : m_calibfile(""), m_index(1), m_num_axes(6), m_num_channels(6), - m_sample_bias() + : m_calibfile(""), m_index(1), m_num_axes(6), m_num_channels(6), m_sample_bias() { } @@ -80,10 +78,8 @@ void vpForceTorqueAtiSensor::bias() m_sample_bias = vpComedi::getPhyData(); if (m_sample_bias.size() != m_num_channels) - throw vpException( - vpException::fatalError, - "Physical data size (%d) and number of channels (%d) doesn't match", - m_sample_bias.size(), m_num_channels); + throw vpException(vpException::fatalError, "Physical data size (%d) and number of channels (%d) doesn't match", + m_sample_bias.size(), m_num_channels); float *sample_bias = new float[m_num_channels]; for (unsigned int i = 0; i < m_num_channels; i++) @@ -109,10 +105,8 @@ void vpForceTorqueAtiSensor::unbias() m_sample_bias = 0; if (m_sample_bias.size() != m_num_channels) - throw vpException( - vpException::fatalError, - "Physical data size (%d) and number of channels (%d) doesn't match", - m_sample_bias.size(), m_num_channels); + throw vpException(vpException::fatalError, "Physical data size (%d) and number of channels (%d) doesn't match", + m_sample_bias.size(), m_num_channels); float *sample_bias = new float[m_num_channels]; for (unsigned int i = 0; i < m_num_channels; i++) @@ -150,10 +144,8 @@ vpColVector vpForceTorqueAtiSensor::getForceTorque() const vpColVector phydata = vpComedi::getPhyData(); if (phydata.size() != m_num_channels) - throw vpException( - vpException::fatalError, - "Physical data size (%d) and number of channels (%d) doesn't match", - phydata.size(), m_num_channels); + throw vpException(vpException::fatalError, "Physical data size (%d) and number of channels (%d) doesn't match", + phydata.size(), m_num_channels); float *voltage = new float[m_num_channels]; float *ft = new float[m_num_axes]; @@ -203,8 +195,7 @@ vpForceTorqueAtiSensor::~vpForceTorqueAtiSensor() { close(); } pattern: FT*.cal. \param index : Index of calibration in file (default = 1). \sa getCalibrationFile(), close() */ -void vpForceTorqueAtiSensor::setCalibrationFile(const std::string &calibfile, - unsigned short index) +void vpForceTorqueAtiSensor::setCalibrationFile(const std::string &calibfile, unsigned short index) { m_calibfile = calibfile; m_index = index; @@ -218,9 +209,7 @@ void vpForceTorqueAtiSensor::setCalibrationFile(const std::string &calibfile, // Create calibration struct s_calibinfo = createCalibration(file, m_index); if (s_calibinfo == NULL) { - throw vpException(vpException::fatalError, - "Calibration file %s couldn't be loaded", - m_calibfile.c_str()); + throw vpException(vpException::fatalError, "Calibration file %s couldn't be loaded", m_calibfile.c_str()); } m_num_channels = s_calibinfo->rt.NumChannels; @@ -254,20 +243,17 @@ std::ostream &operator<<(std::ostream &os, const vpForceTorqueAtiSensor &ati) } // display info from calibration file - os << "Calibration Information for " << ati.m_calibfile << ", index #" - << ati.m_index << ":" << std::endl; + os << "Calibration Information for " << ati.m_calibfile << ", index #" << ati.m_index << ":" << std::endl; os << " Serial: " << s_calibinfo->Serial << std::endl; os << " Body Style: " << s_calibinfo->BodyStyle << std::endl; os << " Calibration: " << s_calibinfo->PartNumber << std::endl; os << " Calibration Date: " << s_calibinfo->CalDate << std::endl; os << " Family: " << s_calibinfo->Family << std::endl; - os << " # Channels: " << s_calibinfo->rt.NumChannels - << std::endl; + os << " # Channels: " << s_calibinfo->rt.NumChannels << std::endl; os << " # Axes: " << s_calibinfo->rt.NumAxes << std::endl; os << " Force Units: " << s_calibinfo->ForceUnits << std::endl; os << " Torque Units: " << s_calibinfo->TorqueUnits << std::endl; - os << "Temperature Compensation: " - << (s_calibinfo->TempCompAvailable ? "Yes" : "No") << std::endl; + os << "Temperature Compensation: " << (s_calibinfo->TempCompAvailable ? "Yes" : "No") << std::endl; // print maximum loads of axes os << "\nRated Loads:" << std::endl; @@ -277,8 +263,7 @@ std::ostream &operator<<(std::ostream &os, const vpForceTorqueAtiSensor &ati) units = s_calibinfo->ForceUnits; } else units = s_calibinfo->TorqueUnits; - os << s_calibinfo->AxisNames[i] << ": " << s_calibinfo->MaxLoads[i] << " " - << units << std::endl; + os << s_calibinfo->AxisNames[i] << ": " << s_calibinfo->MaxLoads[i] << " " << units << std::endl; } // print temperature compensation information, if available diff --git a/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp b/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp index 3e5591bccd..55bf33be2e 100644 --- a/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp +++ b/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp @@ -53,8 +53,7 @@ vp1394CMUGrabber::vp1394CMUGrabber() : index(0), // If a camera was not selected the first one (index = 0) will // be used - _format(-1), _mode(-1), _fps(-1), _modeauto(true), _gain(0), _shutter(0), - _color(vp1394CMUGrabber::UNKNOWN) + _format(-1), _mode(-1), _fps(-1), _modeauto(true), _gain(0), _shutter(0), _color(vp1394CMUGrabber::UNKNOWN) { // public members init = false; @@ -93,25 +92,20 @@ void vp1394CMUGrabber::selectCamera(int cam_id) 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")); + 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")); + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Resolve camera can not be used")); break; } close(); @@ -128,9 +122,7 @@ void vp1394CMUGrabber::initCamera() 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "The is no detected camera")); } camerror = camera->InitCamera(); @@ -138,23 +130,18 @@ void vp1394CMUGrabber::initCamera() switch (camerror) { case CAM_ERROR_NOT_INITIALIZED: vpERROR_TRACE("vp1394CMUGrabber error: No camera selected", index); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, - "The is no selected camera")); + 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")); + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Resolve camera can not be used")); break; } close(); @@ -185,9 +172,7 @@ void vp1394CMUGrabber::initCamera() 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition")); } init = true; @@ -263,9 +248,8 @@ void vp1394CMUGrabber::acquire(vpImage &I) default: close(); vpERROR_TRACE("Format conversion not implemented. Acquisition failed."); - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "Format conversion not implemented. " - "Acquisition failed.")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. " + "Acquisition failed.")); break; }; @@ -328,9 +312,8 @@ void vp1394CMUGrabber::acquire(vpImage &I) default: close(); vpERROR_TRACE("Format conversion not implemented. Acquisition failed."); - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "Format conversion not implemented. " - "Acquisition failed.")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. " + "Acquisition failed.")); break; }; } @@ -348,9 +331,7 @@ void vp1394CMUGrabber::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")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition")); } } @@ -443,8 +424,7 @@ void vp1394CMUGrabber::setGain(unsigned short gain) \sa setAutoShutter(), setShutter() */ -void vp1394CMUGrabber::getShutterMinMax(unsigned short &min, - unsigned short &max) +void vp1394CMUGrabber::getShutterMinMax(unsigned short &min, unsigned short &max) { initCamera(); @@ -588,8 +568,7 @@ void vp1394CMUGrabber::setVideoMode(unsigned long format, unsigned long mode) 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Video mode not supported")); } if (camera->IsAcquiring()) { @@ -599,9 +578,7 @@ void vp1394CMUGrabber::setVideoMode(unsigned long format, unsigned long mode) 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition")); } } @@ -610,8 +587,7 @@ void vp1394CMUGrabber::setVideoMode(unsigned long format, unsigned long mode) 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set video format")); } if (camera->SetVideoMode(_mode) != CAM_SUCCESS) { @@ -619,8 +595,7 @@ void vp1394CMUGrabber::setVideoMode(unsigned long format, unsigned long mode) 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set video mode")); } // start acquisition @@ -629,9 +604,7 @@ void vp1394CMUGrabber::setVideoMode(unsigned long format, unsigned long mode) 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition")); } // Update Image dimension @@ -680,8 +653,7 @@ void vp1394CMUGrabber::setFramerate(unsigned long fps) "by the IEEE 1394 camera number %i for the selected " "image format", index); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "The frame rate is not supported")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The frame rate is not supported")); } if (camera->IsAcquiring()) { @@ -691,9 +663,7 @@ void vp1394CMUGrabber::setFramerate(unsigned long fps) 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition")); } } if (camera->SetVideoFrameRate(_fps) != CAM_SUCCESS) { @@ -701,8 +671,7 @@ void vp1394CMUGrabber::setFramerate(unsigned long fps) 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set video frame rate")); } // start acquisition if (camera->StartImageAcquisition() != CAM_SUCCESS) { @@ -710,9 +679,7 @@ void vp1394CMUGrabber::setFramerate(unsigned long fps) 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition")); } } } diff --git a/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp b/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp index 5bd40e0075..e0630679f7 100644 --- a/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp +++ b/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp @@ -57,27 +57,23 @@ #include 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"}; + "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", + "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", }; /*! @@ -126,8 +122,8 @@ int main() */ vp1394TwoGrabber::vp1394TwoGrabber(bool reset) - : camera(NULL), cameras(NULL), num_cameras(0), camera_id(0), verbose(false), - camIsOpen(NULL), num_buffers(4), // ring buffer size + : 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 , @@ -202,23 +198,25 @@ int main() 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 + 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 #include - #include #include - +specific camera by its GUID: +\code +#include +#include +#include +#include int main() { #if defined(VISP_HAVE_DC1394) vpImage 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 + 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(); @@ -300,16 +298,13 @@ void vp1394TwoGrabber::setCamera(uint64_t cam_id) } } if (is_guid == false) { - std::cout << "Error: The camera with guid 0x" << std::hex << cam_id - << " is not present" << std::endl; + 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; + 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")); + 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 @@ -342,8 +337,7 @@ void vp1394TwoGrabber::getCamera(uint64_t &cam_id) } else { close(); vpERROR_TRACE("No cameras found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No cameras found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No cameras found")); } } @@ -368,8 +362,7 @@ uint64_t vp1394TwoGrabber::getCamera() } else { close(); vpERROR_TRACE("No cameras found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No cameras found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No cameras found")); } } @@ -459,36 +452,30 @@ void vp1394TwoGrabber::setVideoMode(vp1394TwoVideoModeType videomode) open(); if (!num_cameras) { close(); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } if (!isVideoModeSupported(videomode)) { - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Video mode not supported by camera %d", + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Video mode not supported by camera %d", camera_id)); } // Stop dma capture if started setTransmission(DC1394_OFF); setCapture(DC1394_OFF); - if (dc1394_video_set_mode(camera, (dc1394video_mode_t)videomode) != - DC1394_SUCCESS) { + if (dc1394_video_set_mode(camera, (dc1394video_mode_t)videomode) != DC1394_SUCCESS) { close(); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "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) { + if (dc1394_get_image_size_from_video_mode(camera, (dc1394video_mode_t)videomode, &this->width, &this->height) != + DC1394_SUCCESS) { close(); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Can't get image size")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get image size")); } } @@ -513,8 +500,7 @@ void vp1394TwoGrabber::getVideoMode(vp1394TwoVideoModeType &videomode) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } dc1394video_mode_t _videomode; @@ -522,8 +508,7 @@ void vp1394TwoGrabber::getVideoMode(vp1394TwoVideoModeType &videomode) close(); vpERROR_TRACE("Can't get current video mode"); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Can't get current video mode")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get current video mode")); } videomode = (vp1394TwoVideoModeType)_videomode; } @@ -545,8 +530,7 @@ void vp1394TwoGrabber::getVideoMode(vp1394TwoVideoModeType &videomode) \sa setVideoMode(), getVideoMode(), getCamera() */ -uint32_t vp1394TwoGrabber::getVideoModeSupported( - std::list &videomodes) +uint32_t vp1394TwoGrabber::getVideoModeSupported(std::list &videomodes) { // Refresh the list of supported modes videomodes.clear(); @@ -554,25 +538,21 @@ uint32_t vp1394TwoGrabber::getVideoModeSupported( if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "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) { + 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")); + 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]; + vp1394TwoVideoModeType _mode = (vp1394TwoVideoModeType)_videomodes.modes[i]; videomodes.push_back(_mode); } @@ -599,19 +579,16 @@ bool vp1394TwoGrabber::isVideoModeSupported(vp1394TwoVideoModeType videomode) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "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) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get video modes")); } // parse the video modes to check with the desired @@ -710,8 +687,7 @@ void vp1394TwoGrabber::setFramerate(vp1394TwoFramerateType fps) open(); if (!num_cameras) { close(); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } vp1394TwoVideoModeType cur_videomode; @@ -720,8 +696,7 @@ void vp1394TwoGrabber::setFramerate(vp1394TwoFramerateType fps) return; if (!isFramerateSupported(cur_videomode, fps)) { - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Framerate not supported by camera %d", + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Framerate not supported by camera %d", camera_id)); } @@ -729,12 +704,10 @@ void vp1394TwoGrabber::setFramerate(vp1394TwoFramerateType fps) setTransmission(DC1394_OFF); setCapture(DC1394_OFF); - if (dc1394_video_set_framerate(camera, (dc1394framerate_t)fps) != - DC1394_SUCCESS) { + if (dc1394_video_set_framerate(camera, (dc1394framerate_t)fps) != DC1394_SUCCESS) { close(); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Can't set framerate")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set framerate")); } setCapture(DC1394_ON); @@ -762,16 +735,14 @@ void vp1394TwoGrabber::getFramerate(vp1394TwoFramerateType &fps) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get current framerate")); } fps = (vp1394TwoFramerateType)_fps; } @@ -807,14 +778,12 @@ void vp1394TwoGrabber::getFramerate(vp1394TwoFramerateType &fps) \sa setFramerate(), getFramerate(), setCamera() */ -uint32_t vp1394TwoGrabber::getFramerateSupported( - vp1394TwoVideoModeType mode, std::list &fps) +uint32_t vp1394TwoGrabber::getFramerateSupported(vp1394TwoVideoModeType mode, std::list &fps) { if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } // Refresh the list of supported framerates @@ -837,13 +806,10 @@ uint32_t vp1394TwoGrabber::getFramerateSupported( break; default: { dc1394framerates_t _fps; - if (dc1394_video_get_supported_framerates( - camera, (dc1394video_mode_t)mode, &_fps) != DC1394_SUCCESS) { + 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")); + 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; @@ -888,14 +854,12 @@ uint32_t vp1394TwoGrabber::getFramerateSupported( supported. \sa setFramerate(), getFramerate(), setCamera() */ -bool vp1394TwoGrabber::isFramerateSupported(vp1394TwoVideoModeType mode, - vp1394TwoFramerateType fps) +bool vp1394TwoGrabber::isFramerateSupported(vp1394TwoVideoModeType mode, vp1394TwoFramerateType fps) { if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } switch (mode) { @@ -915,13 +879,10 @@ bool vp1394TwoGrabber::isFramerateSupported(vp1394TwoVideoModeType mode, break; default: { dc1394framerates_t _fps; - if (dc1394_video_get_supported_framerates( - camera, (dc1394video_mode_t)mode, &_fps) != DC1394_SUCCESS) { + 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")); + 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; @@ -988,21 +949,18 @@ void vp1394TwoGrabber::setColorCoding(vp1394TwoColorCodingType coding) { if (!num_cameras) { close(); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } dc1394video_mode_t _videomode; if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) { close(); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Can't get current video mode")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get current video mode")); } if (!isColorCodingSupported((vp1394TwoVideoModeType)_videomode, coding)) { - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Color coding not supported by camera %d", + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Color coding not supported by camera %d", camera_id)); } @@ -1011,13 +969,10 @@ void vp1394TwoGrabber::setColorCoding(vp1394TwoColorCodingType coding) setTransmission(DC1394_OFF); setCapture(DC1394_OFF); - if (dc1394_format7_set_color_coding(camera, _videomode, - (dc1394color_coding_t)coding) != - DC1394_SUCCESS) { + if (dc1394_format7_set_color_coding(camera, _videomode, (dc1394color_coding_t)coding) != DC1394_SUCCESS) { close(); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Can't set color coding")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set color coding")); } setCapture(DC1394_ON); @@ -1047,43 +1002,33 @@ void vp1394TwoGrabber::getColorCoding(vp1394TwoColorCodingType &coding) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "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")); + 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) { + 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")); + 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 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) { + 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")); + 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; @@ -1110,14 +1055,13 @@ void vp1394TwoGrabber::getColorCoding(vp1394TwoColorCodingType &coding) \sa setColorCoding(), getColorCoding(), setCamera() */ -uint32_t vp1394TwoGrabber::getColorCodingSupported( - vp1394TwoVideoModeType mode, std::list &codings) +uint32_t vp1394TwoGrabber::getColorCodingSupported(vp1394TwoVideoModeType mode, + std::list &codings) { if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } // Refresh the list of supported framerates @@ -1126,14 +1070,10 @@ uint32_t vp1394TwoGrabber::getColorCodingSupported( 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) { + 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")); + 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; @@ -1148,14 +1088,10 @@ uint32_t vp1394TwoGrabber::getColorCodingSupported( } 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) { + 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")); + 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; @@ -1182,27 +1118,21 @@ uint32_t vp1394TwoGrabber::getColorCodingSupported( not supported. \sa setColorCoding(), getColorCoding(), setCamera() */ -bool vp1394TwoGrabber::isColorCodingSupported(vp1394TwoVideoModeType mode, - vp1394TwoColorCodingType coding) +bool vp1394TwoGrabber::isColorCodingSupported(vp1394TwoVideoModeType mode, vp1394TwoColorCodingType coding) { if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "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) { + 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")); + 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; @@ -1218,14 +1148,10 @@ bool vp1394TwoGrabber::isColorCodingSupported(vp1394TwoVideoModeType mode, } 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) { + 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")); + 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) @@ -1266,15 +1192,13 @@ bool vp1394TwoGrabber::isColorCodingSupported(vp1394TwoVideoModeType mode, \sa isVideoModeFormat7() */ -void vp1394TwoGrabber::setFormat7ROI(unsigned int left, unsigned int top, - unsigned int w, unsigned int h) +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")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } dc1394video_mode_t _videomode; @@ -1282,8 +1206,7 @@ void vp1394TwoGrabber::setFormat7ROI(unsigned int left, unsigned int top, close(); vpERROR_TRACE("Can't get current video mode"); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "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 @@ -1291,13 +1214,11 @@ void vp1394TwoGrabber::setFormat7ROI(unsigned int left, unsigned int top, 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) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get format7 max image size")); } #if 0 vpTRACE("left: %d top: %d width: %d height: %d", left, top, @@ -1308,13 +1229,11 @@ void vp1394TwoGrabber::setFormat7ROI(unsigned int left, unsigned int top, if (left > max_width) { vpERROR_TRACE("Can't set format7 ROI"); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set format7 ROI")); } int32_t roi_width; @@ -1338,26 +1257,22 @@ void vp1394TwoGrabber::setFormat7ROI(unsigned int left, unsigned int top, 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) { + 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")); + 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) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get format7 image size")); } setCapture(DC1394_ON); @@ -1391,17 +1306,14 @@ void vp1394TwoGrabber::initialize(bool reset) dc1394_camera_free_list(list); close(); vpERROR_TRACE("Failed to enumerate cameras\n"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, - "Failed to enumerate cameras")); + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No cameras found")); } if (cameras != NULL) @@ -1414,8 +1326,7 @@ void vp1394TwoGrabber::initialize(bool reset) 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); + vpTRACE("Failed to initialize camera with guid \"%ld\"\n", list->ids[i].guid); continue; } // Update the number of working cameras @@ -1446,17 +1357,14 @@ void vp1394TwoGrabber::initialize(bool reset) " - 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")); + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No cameras found")); } // allocation for the parameters @@ -1471,14 +1379,12 @@ void vp1394TwoGrabber::initialize(bool reset) 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")); + 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 << "Number of camera(s) on the bus : " << num_cameras << std::endl; std::cout << "-----------------------------" << std::endl; } @@ -1509,17 +1415,15 @@ void vp1394TwoGrabber::open() dc1394switch_t status = DC1394_OFF; //#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > - //libdc1394-2.0.0-rc7 + // 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) + 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) + if (dc1394_video_get_transmission(cameras[camera_id], &status) != DC1394_SUCCESS) vpTRACE("Could get ISO status"); else { if (status == DC1394_ON) { @@ -1531,7 +1435,7 @@ void vp1394TwoGrabber::open() #endif } //#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > - //libdc1394-2.0.0-rc7 + // libdc1394-2.0.0-rc7 } //#endif } @@ -1557,8 +1461,7 @@ void vp1394TwoGrabber::close() 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 + this->camera_id = i; // set camera id for the function updateDataStructToCam setTransmission(DC1394_OFF); setCapture(DC1394_OFF); if (isDataModified[i]) { @@ -1568,37 +1471,18 @@ void vp1394TwoGrabber::close() } 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])) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to reset the initial mode")); } } if (dc1394_camera_set_power(camera, DC1394_OFF) != DC1394_SUCCESS) @@ -1670,8 +1554,7 @@ void vp1394TwoGrabber::setRingBufferSize(unsigned int size) { if (size < 1) { close(); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Could not set ring buffer size")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not set ring buffer size")); } if (size != num_buffers) { @@ -1693,10 +1576,7 @@ void vp1394TwoGrabber::setRingBufferSize(unsigned int size) \sa setRingBufferSize() */ -unsigned int vp1394TwoGrabber::getRingBufferSize() const -{ - return num_buffers; -} +unsigned int vp1394TwoGrabber::getRingBufferSize() const { return num_buffers; } /*! Enable auto shutter. It is also possible to set auto shutter min @@ -1742,8 +1622,7 @@ void vp1394TwoGrabber::setAutoShutter(bool enable) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } dc1394feature_mode_t mode; @@ -1753,20 +1632,16 @@ void vp1394TwoGrabber::setAutoShutter(bool enable) mode = DC1394_FEATURE_MODE_MANUAL; } - if (dc1394_feature_set_power(camera, DC1394_FEATURE_SHUTTER, DC1394_ON) != - DC1394_SUCCESS) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set shutter on")); } - if (dc1394_feature_set_mode(camera, DC1394_FEATURE_SHUTTER, mode) != - DC1394_SUCCESS) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set auto shutter")); } } /*! @@ -1809,19 +1684,15 @@ int main() \sa setAutoShutter(), getAutoShutter() */ -void vp1394TwoGrabber::setAutoShutter(unsigned int minvalue, - unsigned int maxvalue) +void vp1394TwoGrabber::setAutoShutter(unsigned int minvalue, unsigned int maxvalue) { setAutoShutter(); - if (dc1394_avt_set_auto_shutter(camera, minvalue, maxvalue) != - DC1394_SUCCESS) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set auto shutter min and max values")); } } @@ -1837,24 +1708,19 @@ void vp1394TwoGrabber::setAutoShutter(unsigned int minvalue, \exception vpFrameGrabberException::initializationError : If no camera found on the bus. */ -void vp1394TwoGrabber::getAutoShutter(unsigned int &minvalue, - unsigned int &maxvalue) +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")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } - if (dc1394_avt_get_auto_shutter(camera, &minvalue, &maxvalue) != - DC1394_SUCCESS) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot get auto shutter min and max values")); } } @@ -1902,8 +1768,7 @@ void vp1394TwoGrabber::setAutoGain(bool enable) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } dc1394feature_mode_t mode; @@ -1913,20 +1778,16 @@ void vp1394TwoGrabber::setAutoGain(bool enable) mode = DC1394_FEATURE_MODE_MANUAL; } - if (dc1394_feature_set_power(camera, DC1394_FEATURE_GAIN, DC1394_ON) != - DC1394_SUCCESS) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set shutter on")); } - if (dc1394_feature_set_mode(camera, DC1394_FEATURE_GAIN, mode) != - DC1394_SUCCESS) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set auto gain")); } } /*! @@ -1969,18 +1830,15 @@ int main() \sa setAutoGain(), getAutoGain() */ -void vp1394TwoGrabber::setAutoGain(unsigned int minvalue, - unsigned int maxvalue) +void vp1394TwoGrabber::setAutoGain(unsigned int minvalue, unsigned int maxvalue) { setAutoGain(); - if (dc1394_avt_set_auto_gain(camera, minvalue, maxvalue) != - DC1394_SUCCESS) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set auto gain min and max values")); } } @@ -1996,23 +1854,19 @@ void vp1394TwoGrabber::setAutoGain(unsigned int minvalue, \exception vpFrameGrabberException::initializationError : If no camera found on the bus. */ -void vp1394TwoGrabber::getAutoGain(unsigned int &minvalue, - unsigned int &maxvalue) +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")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } - if (dc1394_avt_get_auto_gain(camera, &minvalue, &maxvalue) != - DC1394_SUCCESS) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot get auto gain min and max values")); } } @@ -2038,22 +1892,18 @@ void vp1394TwoGrabber::setCapture(dc1394switch_t _switch) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "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) { + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not setup dma capture")); } } else { // _switch == DC1394_OFF dc1394error_t code = dc1394_capture_stop(camera); @@ -2061,8 +1911,7 @@ void vp1394TwoGrabber::setCapture(dc1394switch_t _switch) 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not setup dma capture")); } } } @@ -2086,8 +1935,7 @@ void vp1394TwoGrabber::setTransmission(dc1394switch_t _switch) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } dc1394switch_t status = DC1394_OFF; @@ -2095,8 +1943,7 @@ void vp1394TwoGrabber::setTransmission(dc1394switch_t _switch) 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not setup dma capture")); } // if (status!=_switch){ @@ -2106,8 +1953,7 @@ void vp1394TwoGrabber::setTransmission(dc1394switch_t _switch) "make sure that the video mode and framerate are " "supported by your camera.\n"); close(); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Could not setup dma capture")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not setup dma capture")); } if (_switch == DC1394_ON) { @@ -2119,8 +1965,7 @@ void vp1394TwoGrabber::setTransmission(dc1394switch_t _switch) 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not setup dma capture")); } } } @@ -2166,8 +2011,7 @@ void vp1394TwoGrabber::setIsoTransmissionSpeed(vp1394TwoIsoSpeedType isospeed) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } dc1394operation_mode_t op_mode; @@ -2179,54 +2023,44 @@ void vp1394TwoGrabber::setIsoTransmissionSpeed(vp1394TwoIsoSpeedType isospeed) // 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Camera is not 1394B mode capable")); } - if (dc1394_video_set_operation_mode( - camera, DC1394_OPERATION_MODE_1394B) != DC1394_SUCCESS) { + 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")); + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Failed to set 1394B mode")); } } else { - if (dc1394_video_set_operation_mode( - camera, DC1394_OPERATION_MODE_LEGACY) != DC1394_SUCCESS) { + 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")); + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Failed to set 1394A mode")); } } - if (dc1394_video_set_iso_speed(camera, (dc1394speed_t)isospeed) != - DC1394_SUCCESS) { + 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")); + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Failed to set iso speed")); } } @@ -2306,14 +2140,12 @@ dc1394video_frame_t *vp1394TwoGrabber::dequeue() if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "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) { + if (dc1394_capture_dequeue(camera, DC1394_CAPTURE_POLICY_WAIT, &frame) != DC1394_SUCCESS) { vpERROR_TRACE("Error: Failed to capture from camera %d\n", camera_id); } @@ -2421,9 +2253,7 @@ int main() \sa enqueue() */ -dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage &I, - uint64_t ×tamp, - uint32_t &id) +dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage &I, uint64_t ×tamp, uint32_t &id) { open(); @@ -2447,28 +2277,23 @@ dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage &I, 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)); + 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); + vpImageConvert::MONO16ToGrey((unsigned char *)frame->image, I.bitmap, size); break; case DC1394_COLOR_CODING_YUV411: - vpImageConvert::YUV411ToGrey((unsigned char *)frame->image, I.bitmap, - size); + vpImageConvert::YUV411ToGrey((unsigned char *)frame->image, I.bitmap, size); break; case DC1394_COLOR_CODING_YUV422: - vpImageConvert::YUV422ToGrey((unsigned char *)frame->image, I.bitmap, - size); + vpImageConvert::YUV422ToGrey((unsigned char *)frame->image, I.bitmap, size); break; case DC1394_COLOR_CODING_YUV444: - vpImageConvert::YUV444ToGrey((unsigned char *)frame->image, I.bitmap, - size); + vpImageConvert::YUV444ToGrey((unsigned char *)frame->image, I.bitmap, size); break; case DC1394_COLOR_CODING_RGB8: @@ -2478,9 +2303,8 @@ dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage &I, default: close(); vpERROR_TRACE("Format conversion not implemented. Acquisition failed."); - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "Format conversion not implemented. " - "Acquisition failed.")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. " + "Acquisition failed.")); }; return frame; @@ -2586,9 +2410,7 @@ int main() \sa enqueue() */ -dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage &I, - uint64_t ×tamp, - uint32_t &id) +dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage &I, uint64_t ×tamp, uint32_t &id) { open(); @@ -2612,42 +2434,35 @@ dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage &I, 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); + 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); + 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); + 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); + 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); + 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); + 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.")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. " + "Acquisition failed.")); }; return frame; @@ -2669,8 +2484,7 @@ void vp1394TwoGrabber::enqueue(dc1394video_frame_t *frame) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } if (frame) @@ -2719,8 +2533,7 @@ void vp1394TwoGrabber::acquire(vpImage &I) \sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue() */ -void vp1394TwoGrabber::acquire(vpImage &I, uint64_t ×tamp, - uint32_t &id) +void vp1394TwoGrabber::acquire(vpImage &I, uint64_t ×tamp, uint32_t &id) { dc1394video_frame_t *frame; @@ -2771,8 +2584,7 @@ void vp1394TwoGrabber::acquire(vpImage &I) \sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue() */ -void vp1394TwoGrabber::acquire(vpImage &I, uint64_t ×tamp, - uint32_t &id) +void vp1394TwoGrabber::acquire(vpImage &I, uint64_t ×tamp, uint32_t &id) { dc1394video_frame_t *frame; @@ -2793,36 +2605,30 @@ void vp1394TwoGrabber::acquire(vpImage &I, uint64_t ×tamp, 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); + 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); + 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); + 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); + 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); + 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.")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. " + "Acquisition failed.")); }; enqueue(frame); @@ -2849,8 +2655,7 @@ void vp1394TwoGrabber::getWidth(unsigned int &w) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } w = this->width; @@ -2878,8 +2683,7 @@ unsigned int vp1394TwoGrabber::getWidth() if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } return this->width; @@ -2907,8 +2711,7 @@ void vp1394TwoGrabber::getHeight(unsigned int &h) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } h = this->height; @@ -2935,8 +2738,7 @@ unsigned int vp1394TwoGrabber::getHeight() if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } return this->height; @@ -2949,12 +2751,9 @@ unsigned int vp1394TwoGrabber::getHeight() */ void vp1394TwoGrabber::printCameraInfo() { - std::cout << "----------------------------------------------------------" - << std::endl - << "----- Information for camera " << camera_id - << " -----" << std::endl - << "----------------------------------------------------------" - << std::endl; + 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); @@ -2971,9 +2770,7 @@ void vp1394TwoGrabber::printCameraInfo() { close(); vpERROR_TRACE("unable to get feature set for camera %d\n", camera_id); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Cannot get camera features")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Cannot get camera features")); } else { #ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7 @@ -2982,8 +2779,7 @@ void vp1394TwoGrabber::printCameraInfo() dc1394_print_feature_set(&features); #endif } - std::cout << "----------------------------------------------------------" - << std::endl; + std::cout << "----------------------------------------------------------" << std::endl; } /*! @@ -2998,18 +2794,15 @@ void vp1394TwoGrabber::printCameraInfo() \sa string2videoMode() */ -std::string -vp1394TwoGrabber::videoMode2string(vp1394TwoVideoModeType videomode) +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)) { + 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; + vpCERROR << "The video mode " << (int)videomode << " is not supported by the camera" << std::endl; } return _str; @@ -3035,8 +2828,7 @@ std::string vp1394TwoGrabber::framerate2string(vp1394TwoFramerateType 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; + vpCERROR << "The framerate " << (int)fps << " is not supported by the camera" << std::endl; } return _str; @@ -3054,19 +2846,16 @@ std::string vp1394TwoGrabber::framerate2string(vp1394TwoFramerateType fps) \sa string2colorCoding() */ -std::string -vp1394TwoGrabber::colorCoding2string(vp1394TwoColorCodingType colorcoding) +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)) { + 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; + vpCERROR << "The color coding " << (int)colorcoding << " is not supported by the camera" << std::endl; } return _str; @@ -3089,8 +2878,7 @@ vp1394TwoGrabber::colorCoding2string(vp1394TwoColorCodingType colorcoding) \sa videoMode2string() */ -vp1394TwoGrabber::vp1394TwoVideoModeType -vp1394TwoGrabber::string2videoMode(std::string videomode) +vp1394TwoGrabber::vp1394TwoVideoModeType vp1394TwoGrabber::string2videoMode(std::string videomode) { vp1394TwoVideoModeType _id; @@ -3100,8 +2888,7 @@ vp1394TwoGrabber::string2videoMode(std::string videomode) return _id; }; - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "The required videomode is not valid")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The required videomode is not valid")); return (vp1394TwoVideoModeType)0; } @@ -3123,8 +2910,7 @@ vp1394TwoGrabber::string2videoMode(std::string videomode) \sa framerate2string() */ -vp1394TwoGrabber::vp1394TwoFramerateType -vp1394TwoGrabber::string2framerate(std::string framerate) +vp1394TwoGrabber::vp1394TwoFramerateType vp1394TwoGrabber::string2framerate(std::string framerate) { vp1394TwoFramerateType _id; @@ -3134,8 +2920,7 @@ vp1394TwoGrabber::string2framerate(std::string framerate) return _id; }; - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "The required framerate is not valid")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The required framerate is not valid")); return (vp1394TwoFramerateType)0; } @@ -3157,8 +2942,7 @@ vp1394TwoGrabber::string2framerate(std::string framerate) \sa colorCoding2string() */ -vp1394TwoGrabber::vp1394TwoColorCodingType -vp1394TwoGrabber::string2colorCoding(std::string colorcoding) +vp1394TwoGrabber::vp1394TwoColorCodingType vp1394TwoGrabber::string2colorCoding(std::string colorcoding) { vp1394TwoColorCodingType _id; @@ -3168,8 +2952,7 @@ vp1394TwoGrabber::string2colorCoding(std::string colorcoding) return _id; }; - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "The required color coding is not valid")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The required color coding is not valid")); return (vp1394TwoColorCodingType)0; } @@ -3294,8 +3077,7 @@ void vp1394TwoGrabber::setPanControl(unsigned int panControlValue) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } uint64_t offset = 0x884; uint32_t value = 0x82000000 + (uint32_t)panControlValue; @@ -3304,8 +3086,7 @@ void vp1394TwoGrabber::setPanControl(unsigned int panControlValue) if (err != DC1394_SUCCESS) { vpERROR_TRACE("Unable to set PAN register"); close(); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Unable to set PAN register")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to set PAN register")); } } @@ -3325,19 +3106,16 @@ void vp1394TwoGrabber::setPanControl(unsigned int panControlValue) \sa setParameterValue() */ -unsigned int -vp1394TwoGrabber::getParameterValue(vp1394TwoParametersType param) +unsigned int vp1394TwoGrabber::getParameterValue(vp1394TwoParametersType param) { if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } uint32_t value; - dc1394feature_t feature = - DC1394_FEATURE_BRIGHTNESS; // = (dc1394feature_t)param; + dc1394feature_t feature = DC1394_FEATURE_BRIGHTNESS; // = (dc1394feature_t)param; switch (param) { case vpFEATURE_BRIGHTNESS: feature = DC1394_FEATURE_BRIGHTNESS; @@ -3386,8 +3164,7 @@ vp1394TwoGrabber::getParameterValue(vp1394TwoParametersType param) if (err != DC1394_SUCCESS) { vpERROR_TRACE("Unable to get the information"); close(); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Unable to get the information")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to get the information")); } return (unsigned int)value; } @@ -3413,18 +3190,15 @@ vp1394TwoGrabber::getParameterValue(vp1394TwoParametersType param) \sa getParameterValue() */ -void vp1394TwoGrabber::setParameterValue(vp1394TwoParametersType param, - unsigned int val) +void vp1394TwoGrabber::setParameterValue(vp1394TwoParametersType param, unsigned int val) { if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "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; + dc1394feature_t feature = DC1394_FEATURE_BRIGHTNESS; // = (dc1394feature_t)param; switch (param) { case vpFEATURE_BRIGHTNESS: feature = DC1394_FEATURE_BRIGHTNESS; @@ -3477,9 +3251,8 @@ void vp1394TwoGrabber::setParameterValue(vp1394TwoParametersType param, 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")); + throw( + vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to detect the manual mode information")); } for (unsigned int i = 0; i < modesAvailable.num; i++) { @@ -3494,13 +3267,11 @@ void vp1394TwoGrabber::setParameterValue(vp1394TwoParametersType param, // even after several set /* we update the structure */ updateDataCamToStruct(); - err = dc1394_feature_get_mode(camera, feature, - &(initialShutterMode[camera_id])); + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to get the initial mode")); } isDataModified[camera_id] = true; } @@ -3510,21 +3281,17 @@ void vp1394TwoGrabber::setParameterValue(vp1394TwoParametersType param, if (err != DC1394_SUCCESS) { vpERROR_TRACE("Unable to set the muanual mode"); close(); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Unable to set the manual mode")); + 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")); + 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")); + 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")); } } /*! @@ -3539,8 +3306,7 @@ void vp1394TwoGrabber::getGuid(uint64_t &guid) if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } guid = camera->guid; @@ -3558,8 +3324,7 @@ uint64_t vp1394TwoGrabber::getGuid() if (!num_cameras) { close(); vpERROR_TRACE("No camera found"); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, "No camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found")); } return camera->guid; diff --git a/modules/sensor/src/framegrabber/OpenCV/vpOpenCVGrabber.cpp b/modules/sensor/src/framegrabber/OpenCV/vpOpenCVGrabber.cpp index 99e827c738..93c56381dd 100644 --- a/modules/sensor/src/framegrabber/OpenCV/vpOpenCVGrabber.cpp +++ b/modules/sensor/src/framegrabber/OpenCV/vpOpenCVGrabber.cpp @@ -84,9 +84,8 @@ void vpOpenCVGrabber::open() else { close(); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, - "Initialization not done : camera already used or no camera found")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, + "Initialization not done : camera already used or no camera found")); } } @@ -124,9 +123,7 @@ void vpOpenCVGrabber::acquire(vpImage &I) if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done")); } cvGrabFrame(capture); @@ -148,9 +145,7 @@ void vpOpenCVGrabber::acquire(vpImage &I) if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done")); } cvGrabFrame(capture); @@ -172,9 +167,7 @@ IplImage *vpOpenCVGrabber::acquire() if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done")); } cvGrabFrame(capture); @@ -198,10 +191,7 @@ void vpOpenCVGrabber::close() \param framerate : The value of the framerate is returned here. */ -void vpOpenCVGrabber::getFramerate(double &framerate) -{ - framerate = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS); -} +void vpOpenCVGrabber::getFramerate(double &framerate) { framerate = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS); } /*! Sets the capture frame rate @@ -229,9 +219,8 @@ 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, + "Impossible to set the size of the grabber")); } this->width = w; @@ -252,9 +241,8 @@ 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, + "Impossible to set the size of the grabber")); } this->height = h; @@ -278,8 +266,7 @@ void vpOpenCVGrabber::setDeviceType(int type) { DeviceType = type; - if (DeviceType != 0 && DeviceType != 100 && DeviceType != 200 && - DeviceType != 300) { + if (DeviceType != 0 && DeviceType != 100 && DeviceType != 200 && DeviceType != 300) { vpTRACE("The expected type of device may be unknown."); } } diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowDevice.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowDevice.cpp index efe039ec8d..71fdd57af1 100644 --- a/modules/sensor/src/framegrabber/directshow/vpDirectShowDevice.cpp +++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowDevice.cpp @@ -110,8 +110,7 @@ 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) +VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpDirectShowDevice &dev) { return os << dev.name << std::endl << dev.desc << std::endl << dev.devPath; } diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabber.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabber.cpp index d940cafae1..1d811d9044 100644 --- a/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabber.cpp +++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabber.cpp @@ -57,10 +57,7 @@ unsigned int vpDirectShowGrabber::getWidth() { return grabber->getWidth(); } Constructor. Initializes COM. */ -vpDirectShowGrabber::vpDirectShowGrabber() -{ - grabber = new vpDirectShowGrabberImpl(); -} +vpDirectShowGrabber::vpDirectShowGrabber() { grabber = new vpDirectShowGrabberImpl(); } /*! Destructor @@ -92,10 +89,7 @@ void vpDirectShowGrabber::open(vpImage &I) { grabber->open(); } \exception otherError Can't grab the frame */ -void vpDirectShowGrabber::acquire(vpImage &I) -{ - grabber->acquire(I); -} +void vpDirectShowGrabber::acquire(vpImage &I) { grabber->acquire(I); } /*! Grabs a rgba image from the video stream. @@ -116,10 +110,7 @@ void vpDirectShowGrabber::close() { grabber->close(); } Gets the number of capture devices \return the number of capture devices */ -unsigned int vpDirectShowGrabber::getDeviceNumber() -{ - return grabber->getDeviceNumber(); -} +unsigned int vpDirectShowGrabber::getDeviceNumber() { return grabber->getDeviceNumber(); } /*! Set the device (or camera) from which images will be grabbed. @@ -127,10 +118,7 @@ unsigned int vpDirectShowGrabber::getDeviceNumber() \return true if the device is set successfully, false otherwise. */ -bool vpDirectShowGrabber::setDevice(unsigned int id) -{ - return grabber->setDevice(id); -} +bool vpDirectShowGrabber::setDevice(unsigned int id) { return grabber->setDevice(id); } /*! Displays the list of devices on the standard output @@ -143,8 +131,7 @@ void vpDirectShowGrabber::displayDevices() { grabber->displayDevices(); } \return true if successful */ -bool vpDirectShowGrabber::setImageSize(unsigned int width, - unsigned int height) +bool vpDirectShowGrabber::setImageSize(unsigned int width, unsigned int height) { return grabber->setImageSize(width, height); } @@ -154,10 +141,7 @@ bool vpDirectShowGrabber::setImageSize(unsigned int width, \return true if successful */ -bool vpDirectShowGrabber::setFramerate(double framerate) -{ - return grabber->setFramerate(framerate); -} +bool vpDirectShowGrabber::setFramerate(double framerate) { return grabber->setFramerate(framerate); } /*! Set capture format \param width : Image width in pixel @@ -166,8 +150,7 @@ bool vpDirectShowGrabber::setFramerate(double framerate) \return true if successful */ -bool vpDirectShowGrabber::setFormat(unsigned int width, unsigned int height, - double framerate) +bool vpDirectShowGrabber::setFormat(unsigned int width, unsigned int height, double framerate) { return grabber->setFormat(width, height, framerate); } @@ -178,8 +161,7 @@ bool vpDirectShowGrabber::setFormat(unsigned int width, unsigned int height, pixel \param framerate : Pointer to a variable that receives the framerate in fps */ -void vpDirectShowGrabber::getFormat(unsigned int &width, unsigned int &height, - double &framerate) +void vpDirectShowGrabber::getFormat(unsigned int &width, unsigned int &height, double &framerate) { grabber->getFormat(width, height, framerate); } @@ -189,10 +171,7 @@ void vpDirectShowGrabber::getFormat(unsigned int &width, unsigned int &height, \return true if successful */ -bool vpDirectShowGrabber::getStreamCapabilities() -{ - return grabber->getStreamCapabilities(); -} +bool vpDirectShowGrabber::getStreamCapabilities() { return grabber->getStreamCapabilities(); } /*! Set capture MediaType @@ -201,10 +180,7 @@ bool vpDirectShowGrabber::getStreamCapabilities() \return true if successful */ -bool vpDirectShowGrabber::setMediaType(int mediaTypeID) -{ - return grabber->setMediaType(mediaTypeID); -} +bool vpDirectShowGrabber::setMediaType(int mediaTypeID) { return grabber->setMediaType(mediaTypeID); } /* Get current capture MediaType diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp index d9fe3e3db3..191fe1b3e0 100644 --- a/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp +++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp @@ -75,9 +75,7 @@ vpDirectShowGrabberImpl::vpDirectShowGrabberImpl() if (FAILED(hr = CoInitializeEx(NULL, COINIT_MULTITHREADED))) { std::string err; HRtoStr(err); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Can't initialize COM\n" + err)); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Can't initialize COM\n" + err)); } initCo = true; @@ -114,8 +112,7 @@ void vpDirectShowGrabberImpl::open() if (!init) { std::string err; HRtoStr(err); - throw(vpFrameGrabberException( - vpFrameGrabberException::initializationError, err)); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, err)); } } /*! @@ -165,16 +162,14 @@ bool vpDirectShowGrabberImpl::initDirectShow() return false; // get the current connected media type (needed by the callback) - if (FAILED( - hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType)))) + if (FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType)))) return false; // Gets the various graph's interfaces CComPtr pMediaFilter; pGraph->QueryInterface(IID_IMediaFilter, (void **)&pMediaFilter); - pGraph->QueryInterface(IID_IMediaControl, - reinterpret_cast(&pControl)); + pGraph->QueryInterface(IID_IMediaControl, reinterpret_cast(&pControl)); pGraph->QueryInterface(IID_IMediaEvent, (void **)&pEvent); pMediaFilter->SetSyncSource(NULL); @@ -193,21 +188,18 @@ vpDirectShowGrabberImpl::~vpDirectShowGrabberImpl() { close(); } \param ppVideoInputEnum The video device enumerator \return Was the operation successful */ -bool vpDirectShowGrabberImpl::enumerate( - CComPtr &ppVideoInputEnum) +bool vpDirectShowGrabberImpl::enumerate(CComPtr &ppVideoInputEnum) { CComPtr pDevEnum = NULL; bool res = false; // Enumerate system devices - hr = pDevEnum.CoCreateInstance(CLSID_SystemDeviceEnum, NULL, - CLSCTX_INPROC_SERVER); + 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); + hr = pDevEnum->CreateClassEnumerator(CLSID_VideoInputDeviceCategory, &ppVideoInputEnum, 0); if (hr == S_OK) res = true; @@ -222,8 +214,7 @@ bool vpDirectShowGrabberImpl::enumerate( \param ppVideoInputEnum A video device enumerator \return Was the operation successful */ -bool vpDirectShowGrabberImpl::createDeviceList( - CComPtr &ppVideoInputEnum) +bool vpDirectShowGrabberImpl::createDeviceList(CComPtr &ppVideoInputEnum) { CComPtr pMoniker[10]; // const max devices unsigned long nbMoniker; @@ -231,8 +222,7 @@ bool vpDirectShowGrabberImpl::createDeviceList( ppVideoInputEnum->Reset(); // Enumerates the different inputs - ppVideoInputEnum->Next(10, reinterpret_cast(&pMoniker), - &nbMoniker); + ppVideoInputEnum->Next(10, reinterpret_cast(&pMoniker), &nbMoniker); // if no input device if (nbMoniker == 0) @@ -275,8 +265,7 @@ bool vpDirectShowGrabberImpl::createDeviceList( \param ppDevice The n-th device \return Was the operation successful */ -bool vpDirectShowGrabberImpl::getDevice(unsigned int n, - CComPtr &ppDevice) +bool vpDirectShowGrabberImpl::getDevice(unsigned int n, CComPtr &ppDevice) { // if n is invalid, quit if (n >= nbDevices) @@ -299,8 +288,7 @@ bool vpDirectShowGrabberImpl::getDevice(unsigned int n, // implicit conversion should work ... if (deviceList[n] == vpDirectShowDevice(pMoniker)) { // we get the filter - if (SUCCEEDED(pMoniker->BindToObject(0, 0, IID_IBaseFilter, - (void **)&ppDevice))) { + if (SUCCEEDED(pMoniker->BindToObject(0, 0, IID_IBaseFilter, (void **)&ppDevice))) { // now the device is in use deviceList[n].setInUse(); deviceFound = true; @@ -322,8 +310,7 @@ bool vpDirectShowGrabberImpl::getDevice(unsigned int n, \return number of the found device. If the operation wasn't successfull, n=nbDevices. */ -unsigned int -vpDirectShowGrabberImpl::getFirstUnusedDevice(CComPtr &ppDevice) +unsigned int vpDirectShowGrabberImpl::getFirstUnusedDevice(CComPtr &ppDevice) { unsigned int n = 0; bool found = false; @@ -350,8 +337,7 @@ bool vpDirectShowGrabberImpl::createGraph() { // Create the Capture Graph Builder. - hr = pBuild.CoCreateInstance(CLSID_CaptureGraphBuilder2, 0, - CLSCTX_INPROC_SERVER); + hr = pBuild.CoCreateInstance(CLSID_CaptureGraphBuilder2, 0, CLSCTX_INPROC_SERVER); if (SUCCEEDED(hr)) { // Create the Filter Graph Manager. @@ -373,19 +359,16 @@ bool vpDirectShowGrabberImpl::createGraph() \param ppGrabberFilter The created grabber filter's interface \return Was the operation successful */ -bool vpDirectShowGrabberImpl::createSampleGrabber( - CComPtr &ppGrabberFilter) +bool vpDirectShowGrabberImpl::createSampleGrabber(CComPtr &ppGrabberFilter) { // Creates the sample grabber - hr = ppGrabberFilter.CoCreateInstance(CLSID_SampleGrabber, NULL, - CLSCTX_INPROC_SERVER); + 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(&pGrabberI)); + hr = ppGrabberFilter->QueryInterface(IID_ISampleGrabber, reinterpret_cast(&pGrabberI)); if (FAILED(hr)) return false; @@ -434,26 +417,17 @@ bool vpDirectShowGrabberImpl::checkSourceType(CComPtr &pCapSourcePin) 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 || + 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) { + 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; } @@ -467,18 +441,14 @@ bool vpDirectShowGrabberImpl::checkSourceType(CComPtr &pCapSourcePin) 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; + 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; + << (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 @@ -497,8 +467,7 @@ bool vpDirectShowGrabberImpl::checkSourceType(CComPtr &pCapSourcePin) 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 + sgCB.invertedSource = false; // consider that the image is topdown oriented by default } } @@ -511,8 +480,8 @@ bool vpDirectShowGrabberImpl::checkSourceType(CComPtr &pCapSourcePin) \param pGrabber The grabber \return Was the operation successful */ -bool vpDirectShowGrabberImpl::connectSourceToGrabber( - CComPtr &_pCapSource, CComPtr &_pGrabberFilter) +bool vpDirectShowGrabberImpl::connectSourceToGrabber(CComPtr &_pCapSource, + CComPtr &_pGrabberFilter) { /* //get the capture source's output pin @@ -532,8 +501,7 @@ bool vpDirectShowGrabberImpl::connectSourceToGrabber( //not used anymore, we can release it pGrabberInputPin.Release(); */ - if (FAILED(hr = pBuild->RenderStream(NULL, NULL, _pCapSource, NULL, - _pGrabberFilter))) + if (FAILED(hr = pBuild->RenderStream(NULL, NULL, _pCapSource, NULL, _pGrabberFilter))) return false; /* @@ -544,8 +512,7 @@ bool vpDirectShowGrabberImpl::connectSourceToGrabber( */ // get the Null renderer CComPtr pNull = NULL; - if (FAILED(pNull.CoCreateInstance(CLSID_NullRenderer, NULL, - CLSCTX_INPROC_SERVER))) + if (FAILED(pNull.CoCreateInstance(CLSID_NullRenderer, NULL, CLSCTX_INPROC_SERVER))) return false; /* //get the null renderer's input pin @@ -565,8 +532,7 @@ bool vpDirectShowGrabberImpl::connectSourceToGrabber( // get the capture source's output pin CComPtr pCapSourcePin; - if (FAILED(pBuild->FindPin(_pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, - &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 @@ -619,9 +585,7 @@ void vpDirectShowGrabberImpl::acquire(vpImage &I) { if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done")); } // set the rgbaIm pointer on I (will be filled on the next framegrabber @@ -642,8 +606,7 @@ void vpDirectShowGrabberImpl::acquire(vpImage &I) // 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't grab the frame, callback timeout")); } /*! @@ -658,9 +621,7 @@ void vpDirectShowGrabberImpl::acquire(vpImage &I) { if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done")); } // set the grayIm pointer on I (will be filled on the next framegrabber @@ -681,8 +642,7 @@ void vpDirectShowGrabberImpl::acquire(vpImage &I) // 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't grab the frame, callback timeout")); } /*! @@ -695,9 +655,7 @@ bool vpDirectShowGrabberImpl::setDevice(unsigned int id) { if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done")); } // if n is invalid, or the device is already in use, quit @@ -733,8 +691,7 @@ bool vpDirectShowGrabberImpl::setDevice(unsigned int id) return false; // get the current connected media type (needed by the callback) - if (FAILED(hr = pGrabberI->GetConnectedMediaType( - &(sgCB.connectedMediaType)))) { + if (FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType)))) { return false; } @@ -751,9 +708,7 @@ bool vpDirectShowGrabberImpl::setDevice(unsigned int id) void vpDirectShowGrabberImpl::displayDevices() { if (deviceList == NULL) { - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done")); } for (unsigned int i = 0; i < nbDevices; i++) @@ -782,14 +737,11 @@ void vpDirectShowGrabberImpl::close() /*! Set image size */ -bool vpDirectShowGrabberImpl::setImageSize(unsigned int width, - unsigned int height) +bool vpDirectShowGrabberImpl::setImageSize(unsigned int width, unsigned int height) { if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done")); } return setFormat(width, height, NULL); @@ -802,26 +754,20 @@ bool vpDirectShowGrabberImpl::setFramerate(double framerate) { if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done")); } VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER *)sgCB.connectedMediaType.pbFormat; - return setFormat(pVih->bmiHeader.biWidth, pVih->bmiHeader.biHeight, - framerate); + return setFormat(pVih->bmiHeader.biWidth, pVih->bmiHeader.biHeight, framerate); } /*! Set the capture format */ -bool vpDirectShowGrabberImpl::setFormat(unsigned int width, - unsigned int height, double framerate) +bool vpDirectShowGrabberImpl::setFormat(unsigned int width, unsigned int height, double framerate) { if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done")); } bool found = false; @@ -829,27 +775,24 @@ bool vpDirectShowGrabberImpl::setFormat(unsigned int width, // 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))) + 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))) + 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 pCapSourcePin; - if (FAILED(pBuild->FindPin(pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, - &pCapSourcePin))) + if (FAILED(pBuild->FindPin(pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, &pCapSourcePin))) return false; int iCount = 0, iSize = 0; @@ -880,40 +823,32 @@ bool vpDirectShowGrabberImpl::setFormat(unsigned int width, if ((pmtConfig->majortype == sgCB.connectedMediaType.majortype) && (pmtConfig->subtype == sgCB.connectedMediaType.subtype) && (pmtConfig->formattype == sgCB.connectedMediaType.formattype) && - (pmtConfig->cbFormat >= sizeof(VIDEOINFOHEADER)) && - (pmtConfig->pbFormat != NULL)) { + (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) { + 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))) + 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)))) + 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 + 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 + std::cout << "Current format : " << width << " x " << height << " at " << 10000000 / ActualFrameDuration << " fps" << std::endl << std::endl; pVih->AvgTimePerFrame = ActualFrameDuration; @@ -921,22 +856,18 @@ bool vpDirectShowGrabberImpl::setFormat(unsigned int width, found = true; } } else { - if ((unsigned int)lWidth == width && - (unsigned int)lHeight == height) { + 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))) + 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)))) + 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::cout << "Current format : " << width << " x " << height << " at " + << (10000000 / pVih->AvgTimePerFrame) << " fps" << std::endl << std::endl; } } @@ -952,8 +883,7 @@ bool vpDirectShowGrabberImpl::setFormat(unsigned int width, << " 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::cout << "The " << width << " x " << height << "source image size is not available. " << std::endl << std::endl; return found; @@ -964,15 +894,11 @@ bool vpDirectShowGrabberImpl::setFormat(unsigned int width, \param height : Image height. \param framerate : Framerate acquisition. */ -void vpDirectShowGrabberImpl::getFormat(unsigned int &width, - unsigned int &height, - double &framerate) +void vpDirectShowGrabberImpl::getFormat(unsigned int &width, unsigned int &height, double &framerate) { if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done")); } VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER *)sgCB.connectedMediaType.pbFormat; width = (unsigned int)pVih->bmiHeader.biWidth; @@ -986,19 +912,16 @@ bool vpDirectShowGrabberImpl::getStreamCapabilities() { if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + 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))) + 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; @@ -1020,34 +943,28 @@ bool vpDirectShowGrabberImpl::getStreamCapabilities() // LONG lWidth = pVih->bmiHeader.biWidth; // LONG lHeight = - //pVih->bmiHeader.biHeight; SIZE - //dimensions={lWidth,lHeight}; + // pVih->bmiHeader.biHeight; SIZE + // dimensions={lWidth,lHeight}; // LONGLONG lAvgTimePerFrame = - //pVih->AvgTimePerFrame; + // pVih->AvgTimePerFrame; std::cout << "MediaType : " << iFormat << std::endl; if (pmtConfig->subtype == MEDIASUBTYPE_ARGB32) - std::cout << "subtype (not supported): MEDIASUBTYPE_ARGB32" - << std::endl; + 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; + std::cout << "subtype (not supported): MEDIASUBTYPE_RGB555" << std::endl; else if (pmtConfig->subtype == MEDIASUBTYPE_RGB565) - std::cout << "subtype (not supported): MEDIASUBTYPE_RGB565" - << std::endl; + std::cout << "subtype (not supported): MEDIASUBTYPE_RGB565" << std::endl; else if (pmtConfig->subtype == MEDIASUBTYPE_RGB8) - std::cout << "subtype (not supported): MEDIASUBTYPE_RGB8" - << std::endl; + std::cout << "subtype (not supported): MEDIASUBTYPE_RGB8" << std::endl; else if (pmtConfig->subtype == MEDIASUBTYPE_RGB4) - std::cout << "subtype (not supported): MEDIASUBTYPE_RGB4" - << std::endl; + std::cout << "subtype (not supported): MEDIASUBTYPE_RGB4" << std::endl; else if (pmtConfig->subtype == MEDIASUBTYPE_RGB1) - std::cout << "subtype (not supported): MEDIASUBTYPE_RGB1" - << std::endl; + 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) @@ -1065,23 +982,17 @@ bool vpDirectShowGrabberImpl::getStreamCapabilities() else if ((((pVih->bmiHeader.biCompression & 0xFF000000) >> 24) | ((pVih->bmiHeader.biCompression & 0x00FF0000) >> 8) | ((pVih->bmiHeader.biCompression & 0x0000FF00) << 8) | - ((pVih->bmiHeader.biCompression & 0x000000FF) << 24)) == - 'I420') + ((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::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; /* @@ -1112,19 +1023,16 @@ bool vpDirectShowGrabberImpl::setMediaType(int mediaTypeID) { if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + 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))) + 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; @@ -1135,12 +1043,10 @@ bool vpDirectShowGrabberImpl::setMediaType(int mediaTypeID) 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))) + 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)))) + if (FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType)))) return false; } // Delete the media type when you are done. @@ -1156,23 +1062,19 @@ int vpDirectShowGrabberImpl::getMediaType() { if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "Initialization not done")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done")); } int mediaTypeID = -1; - VIDEOINFOHEADER *pVihConnected = - (VIDEOINFOHEADER *)sgCB.connectedMediaType.pbFormat; + 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))) + 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; @@ -1192,8 +1094,7 @@ int vpDirectShowGrabberImpl::getMediaType() if ((pmtConfig->majortype == sgCB.connectedMediaType.majortype) && (pmtConfig->subtype == sgCB.connectedMediaType.subtype) && (pmtConfig->formattype == sgCB.connectedMediaType.formattype) && - (pmtConfig->cbFormat >= sizeof(VIDEOINFOHEADER)) && - (pmtConfig->pbFormat != NULL)) { + (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) diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp index 46dd32455b..562d159a45 100644 --- a/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp +++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp @@ -48,8 +48,7 @@ Constructor - creates the semaphore */ vpDirectShowSampleGrabberI::vpDirectShowSampleGrabberI() - : acqGrayDemand(false), acqRGBaDemand(false), specialMediaType(false), - invertedSource(false) + : acqGrayDemand(false), acqRGBaDemand(false), specialMediaType(false), invertedSource(false) { // semaphore(0), max value = 1 copySem = CreateSemaphore(NULL, 0, 1, NULL); @@ -64,8 +63,7 @@ vpDirectShowSampleGrabberI::~vpDirectShowSampleGrabberI() CloseHandle(copySem); } -STDMETHODIMP vpDirectShowSampleGrabberI::QueryInterface(REFIID riid, - void **ppvObject) +STDMETHODIMP vpDirectShowSampleGrabberI::QueryInterface(REFIID riid, void **ppvObject) { if (NULL == ppvObject) return E_POINTER; @@ -85,16 +83,14 @@ STDMETHODIMP vpDirectShowSampleGrabberI::QueryInterface(REFIID riid, Called when the input buffer is full. Rq : BufferLen == bmpInfo.biWidth*bmpInfo.biHeight*sizeof(vpRGBa) */ -STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, - long BufferLen) +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(connectedMediaType.pbFormat); + VIDEOINFOHEADER *pVih = reinterpret_cast(connectedMediaType.pbFormat); BITMAPINFOHEADER bmpInfo = pVih->bmiHeader; // if biHeight > 0 and the source is not special @@ -116,8 +112,7 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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(), + vpImageConvert::BGRToRGBa(pBuffer, (unsigned char *)rgbaIm->bitmap, rgbaIm->getWidth(), rgbaIm->getHeight(), flip); // reset the demand boolean acqRGBaDemand = false; @@ -126,29 +121,23 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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); + 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; + 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 (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()); + 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 @@ -156,9 +145,7 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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()); + vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight()); // reset the demand boolean acqGrayDemand = false; } @@ -169,9 +156,8 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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()); + 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 @@ -179,9 +165,7 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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()); + vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight()); // reset the demand boolean acqGrayDemand = false; } @@ -191,9 +175,8 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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()); + 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 @@ -201,22 +184,18 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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()); + 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) { + } 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()); + 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 @@ -224,9 +203,7 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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()); + vpImageConvert::YCbCrToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight()); // reset the demand boolean acqGrayDemand = false; } @@ -236,9 +213,8 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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()); + 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 @@ -246,9 +222,7 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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()); + vpImageConvert::YCbCrToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight()); // reset the demand boolean acqGrayDemand = false; } @@ -258,9 +232,8 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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()); + 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 @@ -268,9 +241,7 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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()); + vpImageConvert::YUV422ToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight()); // reset the demand boolean acqGrayDemand = false; } @@ -282,8 +253,7 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // copy and convert the image // copy(pBuffer ,pBuffer + // 4*rgbaIm->getWidth()*rgbaIm->getHeight(),rgbaIm->bitmap); - memcpy(rgbaIm->bitmap, pBuffer, - 4 * rgbaIm->getWidth() * rgbaIm->getHeight()); + memcpy(rgbaIm->bitmap, pBuffer, 4 * rgbaIm->getWidth() * rgbaIm->getHeight()); // reset the demand boolean acqRGBaDemand = false; } else // if it was a grayscale image demand @@ -291,9 +261,7 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, // 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()); + vpImageConvert::RGBaToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight()); // reset the demand boolean acqGrayDemand = false; } diff --git a/modules/sensor/src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp b/modules/sensor/src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp index e363a7db86..024c26939e 100644 --- a/modules/sensor/src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp +++ b/modules/sensor/src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp @@ -54,8 +54,7 @@ active. */ vpFlyCaptureGrabber::vpFlyCaptureGrabber() - : m_camera(), m_guid(), m_index(0), m_numCameras(0), m_rawImage(), - m_connected(false), m_capture(false) + : m_camera(), m_guid(), m_index(0), m_numCameras(0), m_rawImage(), m_connected(false), m_capture(false) { m_numCameras = this->getNumCameras(); } @@ -127,10 +126,9 @@ int main() g.connect(); FlyCapture2::Camera *handler = g.getCameraHandler(); bool supported = false; - handler->GetVideoModeAndFrameRateInfo(FlyCapture2::VIDEOMODE_1280x960Y8, -FlyCapture2::FRAMERATE_60, &supported); if (supported) - g.setVideoModeAndFrameRate(FlyCapture2::VIDEOMODE_1280x960Y8, -FlyCapture2::FRAMERATE_60); g.startCapture(); + handler->GetVideoModeAndFrameRateInfo(FlyCapture2::VIDEOMODE_1280x960Y8, FlyCapture2::FRAMERATE_60, &supported); + if (supported) + g.setVideoModeAndFrameRate(FlyCapture2::VIDEOMODE_1280x960Y8, FlyCapture2::FRAMERATE_60); g.startCapture(); for(int i=0; i< nframes; i++) { g.acquire(I); @@ -141,7 +139,8 @@ FlyCapture2::FRAMERATE_60); g.startCapture(); The following code shows how to use this function to check if a given -format7 (here MODE_0, PIXEL_FORMAT_MONO8) is supported by the camera: \code +format7 (here MODE_0, PIXEL_FORMAT_MONO8) is supported by the camera: +\code #include int main() @@ -153,8 +152,7 @@ int main() // Query for available Format 7 modes const FlyCapture2::Mode k_fmt7Mode = FlyCapture2::MODE_0; - const FlyCapture2::PixelFormat k_fmt7PixFmt = -FlyCapture2::PIXEL_FORMAT_MONO8; + const FlyCapture2::PixelFormat k_fmt7PixFmt = FlyCapture2::PIXEL_FORMAT_MONO8; FlyCapture2::Format7Info fmt7Info; bool supported; @@ -165,12 +163,12 @@ FlyCapture2::PIXEL_FORMAT_MONO8; return -1; } if (supported) { - std::cout << "Max image pixels: (" << fmt7Info.maxWidth << ", " << -fmt7Info.maxHeight << ")" << std::endl; std::cout << "Image Unit size: (" << -fmt7Info.imageHStepSize << ", " << fmt7Info.imageVStepSize << ")" << -std::endl; std::cout << "Offset Unit size: (" << fmt7Info.offsetHStepSize << -", " << fmt7Info.offsetVStepSize << ")" << std::endl; std::cout << "Pixel -format bitfield: 0x" << fmt7Info.pixelFormatBitField << std::endl; + std::cout << "Max image pixels: (" << fmt7Info.maxWidth << ", " << fmt7Info.maxHeight << ")" << std::endl; + std::cout << "Image Unit size: (" << fmt7Info.imageHStepSize << ", " << fmt7Info.imageVStepSize << ")" + << std::endl; + std::cout << "Offset Unit size: (" << fmt7Info.offsetHStepSize << ", " << fmt7Info.offsetVStepSize << ")" + << std::endl; + std::cout << "Pixel format bitfield: 0x" << fmt7Info.pixelFormatBitField << std::endl; if ( (k_fmt7PixFmt & fmt7Info.pixelFormatBitField) == 0 ) { // Pixel format not supported! @@ -319,8 +317,9 @@ float vpFlyCaptureGrabber::getExposure() \param index : Camera index. The following code shows how to retrieve the serial id of all the cameras -that are connected on the bus. \code #include - +that are connected on the bus. +\code +#include int main() { @@ -329,16 +328,18 @@ int main() unsigned int num_cameras = vpFlyCaptureGrabber::getNumCameras(); for (unsigned int i=0; i= num_cameras) { - throw(vpException( - vpException::badValue, - "The camera with index %u is not present. Only %d cameras connected.", - index, num_cameras)); + throw(vpException(vpException::badValue, "The camera with index %u is not present. Only %d cameras connected.", + index, num_cameras)); } unsigned int serial_id; FlyCapture2::BusManager busMgr; @@ -357,8 +356,7 @@ unsigned int vpFlyCaptureGrabber::getCameraSerial(unsigned int index) error = busMgr.GetCameraSerialNumberFromIndex(index, &serial_id); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot get camera with index %d serial id.", index)); + throw(vpException(vpException::fatalError, "Cannot get camera with index %d serial id.", index)); } return serial_id; } @@ -381,10 +379,8 @@ unsigned int vpFlyCaptureGrabber::getCameraSerial(unsigned int index) void vpFlyCaptureGrabber::setCameraIndex(unsigned int index) { if (index >= m_numCameras) { - throw(vpException( - vpException::badValue, - "The camera with index %u is not present. Only %d cameras connected.", - index, m_numCameras)); + throw(vpException(vpException::badValue, "The camera with index %u is not present. Only %d cameras connected.", + index, m_numCameras)); } m_index = index; @@ -395,7 +391,9 @@ void vpFlyCaptureGrabber::setCameraIndex(unsigned int index) \param serial_id : Camera serial id. The following example shows how to capture images from a camera that has -seial id 15290004. \code #include +seial id 15290004. +\code +#include int main() { @@ -428,9 +426,7 @@ void vpFlyCaptureGrabber::setCameraSerial(unsigned int serial_id) return; } } - throw(vpException(vpException::badValue, - "The camera with serial id %u is not present.", - serial_id)); + throw(vpException(vpException::badValue, "The camera with serial id %u is not present.", serial_id)); } /*! @@ -442,9 +438,8 @@ void vpFlyCaptureGrabber::setCameraSerial(unsigned int serial_id) \param value : value to set. \param prop_value : Switch to affect value to the corresponding variable. */ -void vpFlyCaptureGrabber::setProperty( - const FlyCapture2::PropertyType &prop_type, bool on, bool auto_on, - float value, PropertyValue prop_value) +void vpFlyCaptureGrabber::setProperty(const FlyCapture2::PropertyType &prop_type, bool on, bool auto_on, float value, + PropertyValue prop_value) { this->connect(); @@ -459,16 +454,13 @@ void vpFlyCaptureGrabber::setProperty( prop.absControl = propInfo.absValSupported; switch (prop_value) { case ABS_VALUE: { - float value_ = - (std::max)((std::min)((float)value, (float)propInfo.absMax), - (float)propInfo.absMin); + float value_ = (std::max)((std::min)((float)value, (float)propInfo.absMax), (float)propInfo.absMin); prop.absValue = value_; break; } case VALUE_A: { - unsigned int value_ = (std::max)( - (std::min)((unsigned int)value, (unsigned int)propInfo.max), - (unsigned int)propInfo.min); + unsigned int value_ = + (std::max)((std::min)((unsigned int)value, (unsigned int)propInfo.max), (unsigned int)propInfo.min); prop.valueA = value_; break; } @@ -478,8 +470,7 @@ void vpFlyCaptureGrabber::setProperty( error = m_camera.SetProperty(&prop); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, "Cannot set property %d.", - (int)prop_type)); + throw(vpException(vpException::fatalError, "Cannot set property %d.", (int)prop_type)); } } } @@ -503,10 +494,9 @@ int main() g.setCameraIndex(0); float framerate = g.getFrameRate(); - std::cout << "Cur frame rate: " << std::fixed << std::setprecision(3) << -framerate << " fps" << std::endl; framerate = g.setFrameRate(30); // Set -framerate to 30 fps std::cout << "New frame rate: " << std::fixed << -std::setprecision(3) << framerate << " fps" << std::endl; + std::cout << "Cur frame rate: " << std::fixed << std::setprecision(3) << framerate << " fps" << std::endl; + framerate = g.setFrameRate(30); // Set framerate to 30 fps + std::cout << "New frame rate: " << std::fixed << std::setprecision(3) << framerate << " fps" << std::endl; g.open(I); while (1) @@ -648,13 +638,11 @@ int main() \sa getBrightness() */ -float vpFlyCaptureGrabber::setBrightness(bool brightness_auto, - float brightness_value) +float vpFlyCaptureGrabber::setBrightness(bool brightness_auto, float brightness_value) { this->connect(); - this->setProperty(FlyCapture2::BRIGHTNESS, true, brightness_auto, - brightness_value); + this->setProperty(FlyCapture2::BRIGHTNESS, true, brightness_auto, brightness_value); FlyCapture2::Property prop = this->getProperty(FlyCapture2::BRIGHTNESS); return prop.absValue; } @@ -703,13 +691,11 @@ int main() \sa getExposure() */ -float vpFlyCaptureGrabber::setExposure(bool exposure_on, bool exposure_auto, - float exposure_value) +float vpFlyCaptureGrabber::setExposure(bool exposure_on, bool exposure_auto, float exposure_value) { this->connect(); - this->setProperty(FlyCapture2::AUTO_EXPOSURE, exposure_on, exposure_auto, - exposure_value); + this->setProperty(FlyCapture2::AUTO_EXPOSURE, exposure_on, exposure_auto, exposure_value); FlyCapture2::Property prop = this->getProperty(FlyCapture2::AUTO_EXPOSURE); return prop.absValue; } @@ -739,10 +725,10 @@ int main() float sharpness = g.getSharpness(); std::cout << "Sharpness : " << sharpness << std::endl; - sharpness = g.setSharpness(true, false, 1000); // Turn manual sharpness on -to 1000 std::cout << "Sharpness manual: " << sharpness << std::endl; sharpness -= g.setSharpness(true, true); // Turn auto sharpness on std::cout << -"Sharpness auto : " << sharpness << std::endl; + sharpness = g.setSharpness(true, false, 1000); // Turn manual sharpness on to 1000 + std::cout << "Sharpness manual: " << sharpness << std::endl; + sharpness = g.setSharpness(true, true); // Turn auto sharpness on + std::cout << "Sharpness auto : " << sharpness << std::endl; g.open(I); ... @@ -752,14 +738,11 @@ to 1000 std::cout << "Sharpness manual: " << sharpness << std::endl; sharpness \sa getSharpness() */ -unsigned int vpFlyCaptureGrabber::setSharpness(bool sharpness_on, - bool sharpness_auto, - unsigned int sharpness_value) +unsigned int vpFlyCaptureGrabber::setSharpness(bool sharpness_on, bool sharpness_auto, unsigned int sharpness_value) { this->connect(); - this->setProperty(FlyCapture2::SHARPNESS, sharpness_on, sharpness_auto, - (float)sharpness_value, VALUE_A); + this->setProperty(FlyCapture2::SHARPNESS, sharpness_on, sharpness_auto, (float)sharpness_value, VALUE_A); FlyCapture2::Property prop = this->getProperty(FlyCapture2::SHARPNESS); return prop.valueA; } @@ -768,8 +751,7 @@ unsigned int vpFlyCaptureGrabber::setSharpness(bool sharpness_on, Return property values. \param prop_type : Property type. */ -FlyCapture2::Property -vpFlyCaptureGrabber::getProperty(FlyCapture2::PropertyType prop_type) +FlyCapture2::Property vpFlyCaptureGrabber::getProperty(FlyCapture2::PropertyType prop_type) { this->connect(); @@ -779,8 +761,7 @@ vpFlyCaptureGrabber::getProperty(FlyCapture2::PropertyType prop_type) error = m_camera.GetProperty(&prop); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot get property %d value.", (int)prop_type)); + throw(vpException(vpException::fatalError, "Cannot get property %d value.", (int)prop_type)); } return prop; } @@ -790,8 +771,7 @@ vpFlyCaptureGrabber::getProperty(FlyCapture2::PropertyType prop_type) \param prop_type : Property type. \exception vpException::fatalError : If property type doesn't exist. */ -FlyCapture2::PropertyInfo -vpFlyCaptureGrabber::getPropertyInfo(FlyCapture2::PropertyType prop_type) +FlyCapture2::PropertyInfo vpFlyCaptureGrabber::getPropertyInfo(FlyCapture2::PropertyType prop_type) { this->connect(); @@ -802,8 +782,7 @@ vpFlyCaptureGrabber::getPropertyInfo(FlyCapture2::PropertyType prop_type) error = m_camera.GetPropertyInfo(&propInfo); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, "Cannot get property %d info.", - (int)prop_type)); + throw(vpException(vpException::fatalError, "Cannot get property %d info.", (int)prop_type)); } return propInfo; } @@ -827,8 +806,9 @@ int main() vpFlyCaptureGrabber g; g.setCameraIndex(0); // Default camera is the first on the bus - g.setVideoModeAndFrameRate(FlyCapture2::VIDEOMODE_1280x960Y8, -FlyCapture2::FRAMERATE_60); g.open(I); g.getCameraInfo(std::cout); + g.setVideoModeAndFrameRate(FlyCapture2::VIDEOMODE_1280x960Y8, FlyCapture2::FRAMERATE_60); + g.open(I); + g.getCameraInfo(std::cout); for(int i=0; i< nframes; i++) { g.acquire(I); @@ -837,8 +817,7 @@ FlyCapture2::FRAMERATE_60); g.open(I); g.getCameraInfo(std::cout); } \endcode */ -void vpFlyCaptureGrabber::setVideoModeAndFrameRate( - FlyCapture2::VideoMode video_mode, FlyCapture2::FrameRate frame_rate) +void vpFlyCaptureGrabber::setVideoModeAndFrameRate(FlyCapture2::VideoMode video_mode, FlyCapture2::FrameRate frame_rate) { this->connect(); @@ -846,27 +825,24 @@ void vpFlyCaptureGrabber::setVideoModeAndFrameRate( error = m_camera.SetVideoModeAndFrameRate(video_mode, frame_rate); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot set video mode and framerate.")); + throw(vpException(vpException::fatalError, "Cannot set video mode and framerate.")); } } /*! Return true if video mode and framerate is supported. */ -bool vpFlyCaptureGrabber::isVideoModeAndFrameRateSupported( - FlyCapture2::VideoMode video_mode, FlyCapture2::FrameRate frame_rate) +bool vpFlyCaptureGrabber::isVideoModeAndFrameRateSupported(FlyCapture2::VideoMode video_mode, + FlyCapture2::FrameRate frame_rate) { this->connect(); FlyCapture2::Error error; bool supported = false; - error = m_camera.GetVideoModeAndFrameRateInfo(video_mode, frame_rate, - &supported); + error = m_camera.GetVideoModeAndFrameRateInfo(video_mode, frame_rate, &supported); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot get video mode and framerate.")); + throw(vpException(vpException::fatalError, "Cannot get video mode and framerate.")); } return supported; } @@ -876,9 +852,8 @@ bool vpFlyCaptureGrabber::isVideoModeAndFrameRateSupported( \param size : Horizontal or vertical roi size. If set to 0, use the max allowed size. \param max_size : Allowed max size. \param step : Step. */ -std::pair -vpFlyCaptureGrabber::centerRoi(unsigned int size, unsigned int max_size, - unsigned int step) +std::pair vpFlyCaptureGrabber::centerRoi(unsigned int size, unsigned int max_size, + unsigned int step) { if (size == 0 || size > max_size) size = max_size; @@ -914,8 +889,7 @@ int main() vpFlyCaptureGrabber g; g.setCameraIndex(0); - g.setFormat7VideoMode(FlyCapture2::MODE_0, FlyCapture2::PIXEL_FORMAT_MONO8, -640, 480); + g.setFormat7VideoMode(FlyCapture2::MODE_0, FlyCapture2::PIXEL_FORMAT_MONO8, 640, 480); g.open(I); ... @@ -923,9 +897,8 @@ int main() } \endcode */ -void vpFlyCaptureGrabber::setFormat7VideoMode( - FlyCapture2::Mode format7_mode, FlyCapture2::PixelFormat pixel_format, - unsigned int width, unsigned int height) +void vpFlyCaptureGrabber::setFormat7VideoMode(FlyCapture2::Mode format7_mode, FlyCapture2::PixelFormat pixel_format, + unsigned int width, unsigned int height) { this->connect(); @@ -940,18 +913,15 @@ void vpFlyCaptureGrabber::setFormat7VideoMode( throw(vpException(vpException::fatalError, "Cannot get format7 info.")); } if (!fmt7_supported) { - throw(vpException(vpException::fatalError, - "Format7 mode %d not supported.", (int)format7_mode)); + throw(vpException(vpException::fatalError, "Format7 mode %d not supported.", (int)format7_mode)); } FlyCapture2::Format7ImageSettings fmt7_settings; fmt7_settings.mode = format7_mode; fmt7_settings.pixelFormat = pixel_format; // Set centered roi - std::pair roi_w = - this->centerRoi(width, fmt7_info.maxWidth, fmt7_info.imageHStepSize); - std::pair roi_h = - this->centerRoi(height, fmt7_info.maxHeight, fmt7_info.imageVStepSize); + std::pair roi_w = this->centerRoi(width, fmt7_info.maxWidth, fmt7_info.imageHStepSize); + std::pair roi_h = this->centerRoi(height, fmt7_info.maxHeight, fmt7_info.imageVStepSize); fmt7_settings.width = roi_w.first; fmt7_settings.offsetX = roi_w.second; fmt7_settings.height = roi_h.first; @@ -960,23 +930,18 @@ void vpFlyCaptureGrabber::setFormat7VideoMode( // Validate the settings FlyCapture2::Format7PacketInfo fmt7_packet_info; bool valid = false; - error = m_camera.ValidateFormat7Settings(&fmt7_settings, &valid, - &fmt7_packet_info); + error = m_camera.ValidateFormat7Settings(&fmt7_settings, &valid, &fmt7_packet_info); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot validate format7 settings.")); + throw(vpException(vpException::fatalError, "Cannot validate format7 settings.")); } if (!valid) { - throw(vpException(vpException::fatalError, - "Format7 settings are not valid.")); + throw(vpException(vpException::fatalError, "Format7 settings are not valid.")); } - error = m_camera.SetFormat7Configuration( - &fmt7_settings, fmt7_packet_info.recommendedBytesPerPacket); + error = m_camera.SetFormat7Configuration(&fmt7_settings, fmt7_packet_info.recommendedBytesPerPacket); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw( - vpException(vpException::fatalError, "Cannot set format7 settings.")); + throw(vpException(vpException::fatalError, "Cannot set format7 settings.")); } } @@ -1015,8 +980,7 @@ void vpFlyCaptureGrabber::startCapture() error = m_camera.StartCapture(); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot start capture for camera with serial %u", + throw(vpException(vpException::fatalError, "Cannot start capture for camera with serial %u", getCameraSerial(m_index))); } m_capture = true; @@ -1061,8 +1025,7 @@ void vpFlyCaptureGrabber::connect() FlyCapture2::Error error; m_numCameras = this->getNumCameras(); if (m_numCameras == 0) { - throw( - vpException(vpException::fatalError, "No camera found on the bus")); + throw(vpException(vpException::fatalError, "No camera found on the bus")); } FlyCapture2::BusManager busMgr; @@ -1070,17 +1033,13 @@ void vpFlyCaptureGrabber::connect() error = busMgr.GetCameraFromIndex(m_index, &m_guid); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot retrieve guid of camera with index %u.", - m_index)); + throw(vpException(vpException::fatalError, "Cannot retrieve guid of camera with index %u.", m_index)); } // Connect to a camera error = m_camera.Connect(&m_guid); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot connect to camera with serial %u", - getCameraSerial(m_index))); + throw(vpException(vpException::fatalError, "Cannot connect to camera with serial %u", getCameraSerial(m_index))); } m_connected = true; } @@ -1151,8 +1110,7 @@ void vpFlyCaptureGrabber::acquire(vpImage &I) \param timestamp : The acquisition timestamp. */ -void vpFlyCaptureGrabber::acquire(vpImage &I, - FlyCapture2::TimeStamp ×tamp) +void vpFlyCaptureGrabber::acquire(vpImage &I, FlyCapture2::TimeStamp ×tamp) { this->open(); @@ -1161,8 +1119,7 @@ void vpFlyCaptureGrabber::acquire(vpImage &I, error = m_camera.RetrieveBuffer(&m_rawImage); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot retrieve image from camera with serial %u", + throw(vpException(vpException::fatalError, "Cannot retrieve image from camera with serial %u", getCameraSerial(m_index))); } timestamp = m_rawImage.GetTimeStamp(); @@ -1175,17 +1132,14 @@ void vpFlyCaptureGrabber::acquire(vpImage &I, // char) * width`, which makes sure there is no paddings or holes // between pixel data. And the convertedImage object is sharing the // same data buffer with vpImage object `I`. - FlyCapture2::Image convertedImage( - height, width, sizeof(unsigned char) * width, I.bitmap, - sizeof(unsigned char) * I.getSize(), FlyCapture2::PIXEL_FORMAT_MONO8); + FlyCapture2::Image convertedImage(height, width, sizeof(unsigned char) * width, I.bitmap, + sizeof(unsigned char) * I.getSize(), FlyCapture2::PIXEL_FORMAT_MONO8); // Convert the raw image - error = - m_rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &convertedImage); + error = m_rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &convertedImage); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot convert image from camera with serial %u", + throw(vpException(vpException::fatalError, "Cannot convert image from camera with serial %u", getCameraSerial(m_index))); } } @@ -1208,8 +1162,7 @@ void vpFlyCaptureGrabber::acquire(vpImage &I) \param timestamp : The acquisition timestamp. */ -void vpFlyCaptureGrabber::acquire(vpImage &I, - FlyCapture2::TimeStamp ×tamp) +void vpFlyCaptureGrabber::acquire(vpImage &I, FlyCapture2::TimeStamp ×tamp) { this->open(); @@ -1218,8 +1171,7 @@ void vpFlyCaptureGrabber::acquire(vpImage &I, error = m_camera.RetrieveBuffer(&m_rawImage); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot retrieve image from camera with serial %u", + throw(vpException(vpException::fatalError, "Cannot retrieve image from camera with serial %u", getCameraSerial(m_index))); } timestamp = m_rawImage.GetTimeStamp(); @@ -1231,8 +1183,7 @@ void vpFlyCaptureGrabber::acquire(vpImage &I, error = m_rawImage.Convert(FlyCapture2::PIXEL_FORMAT_RGBU, &convertedImage); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot convert image from camera with serial %u", + throw(vpException(vpException::fatalError, "Cannot convert image from camera with serial %u", getCameraSerial(m_index))); } height = convertedImage.GetRows(); @@ -1353,8 +1304,7 @@ int main() g.connect(); bool power = g.getCameraPower(); - std::cout << "Camera is powered: " << ((power == true) ? "on" : "off") << -std::endl; + std::cout << "Camera is powered: " << ((power == true) ? "on" : "off") << std::endl; if (power) g.setCameraPower(false); // Power off the camera @@ -1369,8 +1319,7 @@ void vpFlyCaptureGrabber::setCameraPower(bool on) this->connect(); if (!isCameraPowerAvailable()) { - throw(vpException(vpException::badValue, - "Cannot power on camera. Feature not available")); + throw(vpException(vpException::badValue, "Cannot power on camera. Feature not available")); } // Power on the camera @@ -1383,8 +1332,7 @@ void vpFlyCaptureGrabber::setCameraPower(bool on) error = m_camera.WriteRegister(powerReg, powerRegVal); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw( - vpException(vpException::fatalError, "Cannot power on the camera.")); + throw(vpException(vpException::fatalError, "Cannot power on the camera.")); } const unsigned int millisecondsToSleep = 100; @@ -1400,8 +1348,7 @@ void vpFlyCaptureGrabber::setCameraPower(bool on) // register reads during power-up } else if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot power on the camera.")); + throw(vpException(vpException::fatalError, "Cannot power on the camera.")); } retries--; @@ -1410,8 +1357,7 @@ void vpFlyCaptureGrabber::setCameraPower(bool on) // Check for timeout errors after retrying if (error == FlyCapture2::PGRERROR_TIMEOUT) { error.PrintErrorTrace(); - throw(vpException(vpException::fatalError, - "Cannot power on the camera. Timeout occur")); + throw(vpException(vpException::fatalError, "Cannot power on the camera. Timeout occur")); } } @@ -1431,8 +1377,7 @@ int main() } \endcode */ -vpFlyCaptureGrabber &vpFlyCaptureGrabber:: -operator>>(vpImage &I) +vpFlyCaptureGrabber &vpFlyCaptureGrabber::operator>>(vpImage &I) { this->acquire(I); return *this; diff --git a/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.cpp b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.cpp index 520770b810..8a9b14ce0f 100644 --- a/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.cpp +++ b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.cpp @@ -52,8 +52,7 @@ Default constructor that consider the first camera found on the bus as active. */ -vpPylonGrabberGigE::vpPylonGrabberGigE() - : m_camera(), m_index(0), m_numCameras(0), m_connected(false) +vpPylonGrabberGigE::vpPylonGrabberGigE() : m_camera(), m_index(0), m_numCameras(0), m_connected(false) { getNumCameras(); } @@ -99,10 +98,8 @@ std::ostream &vpPylonGrabberGigE::getCameraInfo(std::ostream &os) os << " Serial number : " << deviceInfo.GetSerialNumber() << std::endl; os << " Camera model : " << deviceInfo.GetModelName() << std::endl; os << " Camera vendor : " << deviceInfo.GetVendorName() << std::endl; - os << " Resolution : " << widthMax->GetValue() << "x" - << heightMax->GetValue() << std::endl; - os << " Firmware version : " << deviceInfo.GetDeviceVersion() - << std::endl; + os << " Resolution : " << widthMax->GetValue() << "x" << heightMax->GetValue() << std::endl; + os << " Firmware version : " << deviceInfo.GetDeviceVersion() << std::endl; return os; } @@ -153,8 +150,7 @@ float vpPylonGrabberGigE::getGain() else if (GenApi::IsReadable(m_camera.GainRaw)) return m_camera.GainRaw.GetValue(); else - throw vpException(vpException::notImplementedError, - "Don't know how to get gain."); + throw vpException(vpException::notImplementedError, "Don't know how to get gain."); } /*! @@ -177,8 +173,7 @@ float vpPylonGrabberGigE::getBlackLevel() else if (GenApi::IsReadable(m_camera.BlackLevelRaw)) return m_camera.BlackLevelRaw.GetValue(); else - throw vpException(vpException::notImplementedError, - "Don't know how to get blacklevel."); + throw vpException(vpException::notImplementedError, "Don't know how to get blacklevel."); } /*! @@ -201,8 +196,7 @@ float vpPylonGrabberGigE::getExposure() else if (GenApi::IsReadable(m_camera.ExposureTimeRaw)) return m_camera.ExposureTimeRaw.GetValue(); else - throw vpException(vpException::notImplementedError, - "Don't know how to get exposure."); + throw vpException(vpException::notImplementedError, "Don't know how to get exposure."); } /*! @@ -230,10 +224,8 @@ std::string vpPylonGrabberGigE::getCameraSerial(unsigned int index) getNumCameras(); if (index >= m_numCameras) { - throw(vpException( - vpException::badValue, - "The camera with index %u is not present. Only %d cameras connected.", - index, m_numCameras)); + throw(vpException(vpException::badValue, "The camera with index %u is not present. Only %d cameras connected.", + index, m_numCameras)); } Pylon::CTlFactory &TlFactory = Pylon::CTlFactory::GetInstance(); @@ -282,8 +274,7 @@ vpPylonGrabber::UserSetName vpPylonGrabberGigE::getUserSetDefault() { connect(); - Basler_GigECamera::UserSetDefaultSelectorEnums user_set = - m_camera.UserSetDefaultSelector.GetValue(); + Basler_GigECamera::UserSetDefaultSelectorEnums user_set = m_camera.UserSetDefaultSelector.GetValue(); switch (user_set) { case Basler_GigECamera::UserSetDefaultSelector_Default: @@ -322,10 +313,8 @@ vpPylonGrabber::UserSetName vpPylonGrabberGigE::getUserSetDefault() void vpPylonGrabberGigE::setCameraIndex(unsigned int index) { if (index >= m_numCameras) { - throw(vpException( - vpException::badValue, - "The camera with index %u is not present. Only %d cameras connected.", - index, m_numCameras)); + throw(vpException(vpException::badValue, "The camera with index %u is not present. Only %d cameras connected.", + index, m_numCameras)); } m_index = index; @@ -346,9 +335,7 @@ void vpPylonGrabberGigE::setCameraSerial(const std::string &serial) return; } } - throw(vpException(vpException::badValue, - "The camera with serial id %s is not present.", - serial.c_str())); + throw(vpException(vpException::badValue, "The camera with serial id %s is not present.", serial.c_str())); } /*! @@ -396,8 +383,7 @@ float vpPylonGrabberGigE::setGain(bool gain_auto, float gain_value) m_camera.GainRaw.SetValue(gain_value); return m_camera.GainRaw.GetValue(); } else - throw vpException(vpException::notImplementedError, - "Don't know how to set gain."); + throw vpException(vpException::notImplementedError, "Don't know how to set gain."); } /*! @@ -425,8 +411,7 @@ float vpPylonGrabberGigE::setBlackLevel(float blacklevel_value) m_camera.BlackLevelRaw.SetValue(blacklevel_value); return m_camera.BlackLevelRaw.GetValue(); } else - throw vpException(vpException::notImplementedError, - "Don't know how to set blacklevel."); + throw vpException(vpException::notImplementedError, "Don't know how to set blacklevel."); } /*! @@ -446,8 +431,7 @@ exposure applying \e exposure_value parameter. https://www.ptgrey.com/kb/11020?countryid=237 \sa getExposure() */ -float vpPylonGrabberGigE::setExposure(bool exposure_on, bool exposure_auto, - float exposure_value) +float vpPylonGrabberGigE::setExposure(bool exposure_on, bool exposure_auto, float exposure_value) { connect(); @@ -457,8 +441,7 @@ float vpPylonGrabberGigE::setExposure(bool exposure_on, bool exposure_auto, m_camera.ExposureMode.SetValue(Basler_GigECamera::ExposureMode_Off); if (exposure_auto) - m_camera.ExposureAuto.SetValue( - Basler_GigECamera::ExposureAuto_Continuous); + m_camera.ExposureAuto.SetValue(Basler_GigECamera::ExposureAuto_Continuous); else m_camera.ExposureAuto.SetValue(Basler_GigECamera::ExposureAuto_Off); @@ -469,8 +452,7 @@ float vpPylonGrabberGigE::setExposure(bool exposure_on, bool exposure_auto, m_camera.ExposureTimeRaw.SetValue(exposure_value); return m_camera.ExposureTimeRaw.GetValue(); } else - throw vpException(vpException::notImplementedError, - "Don't know how to set exposure."); + throw vpException(vpException::notImplementedError, "Don't know how to set exposure."); } /*! @@ -496,8 +478,7 @@ float vpPylonGrabberGigE::setGamma(bool gamma_on, float gamma_value) m_camera.Gamma.SetValue(gamma_value); return m_camera.Gamma.GetValue(); } else - throw vpException(vpException::notImplementedError, - "Don't know how to set gamma."); + throw vpException(vpException::notImplementedError, "Don't know how to set gamma."); } /*! @@ -544,23 +525,19 @@ bool vpPylonGrabberGigE::setUserSetDefault(UserSetName user_set) switch (user_set) { case USERSET_DEFAULT: - m_camera.UserSetDefaultSelector.SetValue( - Basler_GigECamera::UserSetDefaultSelector_Default); + m_camera.UserSetDefaultSelector.SetValue(Basler_GigECamera::UserSetDefaultSelector_Default); return true; break; case USERSET_USERSET1: - m_camera.UserSetDefaultSelector.SetValue( - Basler_GigECamera::UserSetDefaultSelector_UserSet1); + m_camera.UserSetDefaultSelector.SetValue(Basler_GigECamera::UserSetDefaultSelector_UserSet1); return true; break; case USERSET_USERSET2: - m_camera.UserSetDefaultSelector.SetValue( - Basler_GigECamera::UserSetDefaultSelector_UserSet2); + m_camera.UserSetDefaultSelector.SetValue(Basler_GigECamera::UserSetDefaultSelector_UserSet2); return true; break; case USERSET_USERSET3: - m_camera.UserSetDefaultSelector.SetValue( - Basler_GigECamera::UserSetDefaultSelector_UserSet3); + m_camera.UserSetDefaultSelector.SetValue(Basler_GigECamera::UserSetDefaultSelector_UserSet3); return true; break; default: @@ -678,8 +655,7 @@ void vpPylonGrabberGigE::acquire(vpImage &I) Pylon::CGrabResultPtr grabResult; // Retrieve an image if (!m_camera.RetrieveResult(2000, grabResult)) { - throw(vpException(vpException::fatalError, - "Cannot retrieve image from camera with serial %s", + throw(vpException(vpException::fatalError, "Cannot retrieve image from camera with serial %s", getCameraSerial(m_index).c_str())); } @@ -692,8 +668,7 @@ void vpPylonGrabberGigE::acquire(vpImage &I) imageConvert.OutputPixelFormat = Pylon::PixelType_Mono8; imageConvert.OutputPaddingX = 0; // Create a converted image - imageConvert.Convert(I.bitmap, sizeof(unsigned char) * width * height, - (Pylon::IImage &)grabResult); + imageConvert.Convert(I.bitmap, sizeof(unsigned char) * width * height, (Pylon::IImage &)grabResult); } } @@ -709,8 +684,7 @@ void vpPylonGrabberGigE::acquire(vpImage &I) Pylon::CGrabResultPtr grabResult; // Retrieve an image if (!m_camera.RetrieveResult(2000, grabResult)) { - throw(vpException(vpException::fatalError, - "Cannot retrieve image from camera with serial %s", + throw(vpException(vpException::fatalError, "Cannot retrieve image from camera with serial %s", getCameraSerial(m_index).c_str())); } @@ -789,23 +763,19 @@ bool vpPylonGrabberGigE::selectUserSet(UserSetName user_set) switch (user_set) { case USERSET_DEFAULT: - m_camera.UserSetSelector.SetValue( - Basler_GigECamera::UserSetSelector_Default); + m_camera.UserSetSelector.SetValue(Basler_GigECamera::UserSetSelector_Default); return true; break; case USERSET_USERSET1: - m_camera.UserSetSelector.SetValue( - Basler_GigECamera::UserSetSelector_UserSet1); + m_camera.UserSetSelector.SetValue(Basler_GigECamera::UserSetSelector_UserSet1); return true; break; case USERSET_USERSET2: - m_camera.UserSetSelector.SetValue( - Basler_GigECamera::UserSetSelector_UserSet2); + m_camera.UserSetSelector.SetValue(Basler_GigECamera::UserSetSelector_UserSet2); return true; break; case USERSET_USERSET3: - m_camera.UserSetSelector.SetValue( - Basler_GigECamera::UserSetSelector_UserSet3); + m_camera.UserSetSelector.SetValue(Basler_GigECamera::UserSetSelector_UserSet3); return true; break; default: diff --git a/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.h b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.h index 8d1c1ece80..b746693984 100644 --- a/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.h +++ b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.h @@ -102,8 +102,7 @@ class VISP_EXPORT vpPylonGrabberGigE : public vpPylonGrabber float setBlackLevel(float blacklevel_value = 0); void setCameraIndex(unsigned int index); void setCameraSerial(const std::string &serial); - float setExposure(bool exposure_on, bool exposure_auto, - float exposure_value = 0); + float setExposure(bool exposure_on, bool exposure_auto, float exposure_value = 0); float setGain(bool gain_auto, float gain_value = 0); float setFrameRate(float frame_rate); float setGamma(bool gamma_on, float gamma_value = 1); @@ -120,8 +119,8 @@ class VISP_EXPORT vpPylonGrabberGigE : public vpPylonGrabber private: Pylon::CBaslerGigEInstantCamera m_camera; //!< Pointer to each camera unsigned int m_index; //!< Active camera index - unsigned int m_numCameras; //!< Number of connected GigE cameras - bool m_connected; //!< true if camera connected + unsigned int m_numCameras; //!< Number of connected GigE cameras + bool m_connected; //!< true if camera connected }; #endif // #ifdef VISP_HAVE_PYLON diff --git a/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.cpp b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.cpp index 481aa618df..c84226bf08 100644 --- a/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.cpp +++ b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.cpp @@ -52,8 +52,7 @@ Default constructor that consider the first camera found on the bus as active. */ -vpPylonGrabberUsb::vpPylonGrabberUsb() - : m_camera(), m_index(0), m_numCameras(0), m_connected(false) +vpPylonGrabberUsb::vpPylonGrabberUsb() : m_camera(), m_index(0), m_numCameras(0), m_connected(false) { getNumCameras(); } @@ -99,10 +98,8 @@ std::ostream &vpPylonGrabberUsb::getCameraInfo(std::ostream &os) os << " Serial number : " << deviceInfo.GetSerialNumber() << std::endl; os << " Camera model : " << deviceInfo.GetModelName() << std::endl; os << " Camera vendor : " << deviceInfo.GetVendorName() << std::endl; - os << " Resolution : " << widthMax->GetValue() << "x" - << heightMax->GetValue() << std::endl; - os << " Firmware version : " << deviceInfo.GetDeviceVersion() - << std::endl; + os << " Resolution : " << widthMax->GetValue() << "x" << heightMax->GetValue() << std::endl; + os << " Firmware version : " << deviceInfo.GetDeviceVersion() << std::endl; return os; } @@ -151,8 +148,7 @@ float vpPylonGrabberUsb::getGain() if (GenApi::IsReadable(m_camera.Gain)) return m_camera.Gain.GetValue(); else - throw vpException(vpException::notImplementedError, - "Don't know how to get gain."); + throw vpException(vpException::notImplementedError, "Don't know how to get gain."); } /*! @@ -173,8 +169,7 @@ float vpPylonGrabberUsb::getBlackLevel() if (GenApi::IsReadable(m_camera.BlackLevel)) return m_camera.BlackLevel.GetValue(); else - throw vpException(vpException::notImplementedError, - "Don't know how to get blacklevel."); + throw vpException(vpException::notImplementedError, "Don't know how to get blacklevel."); } /*! @@ -195,8 +190,7 @@ float vpPylonGrabberUsb::getExposure() if (GenApi::IsReadable(m_camera.ExposureTime)) return m_camera.ExposureTime.GetValue() * 0.001; else - throw vpException(vpException::notImplementedError, - "Don't know how to get exposure."); + throw vpException(vpException::notImplementedError, "Don't know how to get exposure."); } /*! @@ -224,10 +218,8 @@ std::string vpPylonGrabberUsb::getCameraSerial(unsigned int index) getNumCameras(); if (index >= m_numCameras) { - throw(vpException( - vpException::badValue, - "The camera with index %u is not present. Only %d cameras connected.", - index, m_numCameras)); + throw(vpException(vpException::badValue, "The camera with index %u is not present. Only %d cameras connected.", + index, m_numCameras)); } Pylon::CTlFactory &TlFactory = Pylon::CTlFactory::GetInstance(); @@ -276,8 +268,7 @@ vpPylonGrabber::UserSetName vpPylonGrabberUsb::getUserSetDefault() { connect(); - Basler_UsbCameraParams::UserSetDefaultEnums user_set = - m_camera.UserSetDefault.GetValue(); + Basler_UsbCameraParams::UserSetDefaultEnums user_set = m_camera.UserSetDefault.GetValue(); switch (user_set) { case Basler_UsbCameraParams::UserSetDefault_Default: @@ -316,10 +307,8 @@ vpPylonGrabber::UserSetName vpPylonGrabberUsb::getUserSetDefault() void vpPylonGrabberUsb::setCameraIndex(unsigned int index) { if (index >= m_numCameras) { - throw(vpException( - vpException::badValue, - "The camera with index %u is not present. Only %d cameras connected.", - index, m_numCameras)); + throw(vpException(vpException::badValue, "The camera with index %u is not present. Only %d cameras connected.", + index, m_numCameras)); } m_index = index; @@ -340,9 +329,7 @@ void vpPylonGrabberUsb::setCameraSerial(const std::string &serial) return; } } - throw(vpException(vpException::badValue, - "The camera with serial id %s is not present.", - serial.c_str())); + throw(vpException(vpException::badValue, "The camera with serial id %s is not present.", serial.c_str())); } /*! @@ -387,8 +374,7 @@ float vpPylonGrabberUsb::setGain(bool gain_auto, float gain_value) m_camera.Gain.SetValue(gain_value); return m_camera.Gain.GetValue(); } else - throw vpException(vpException::notImplementedError, - "Don't know how to set gain."); + throw vpException(vpException::notImplementedError, "Don't know how to set gain."); } /*! @@ -413,8 +399,7 @@ float vpPylonGrabberUsb::setBlackLevel(float blacklevel_value) m_camera.BlackLevel.SetValue(blacklevel_value); return m_camera.BlackLevel.GetValue(); } else - throw vpException(vpException::notImplementedError, - "Don't know how to set blacklevel."); + throw vpException(vpException::notImplementedError, "Don't know how to set blacklevel."); } /*! @@ -434,21 +419,17 @@ exposure applying \e exposure_value parameter. https://www.ptgrey.com/kb/11020?countryid=237 \sa getExposure() */ -float vpPylonGrabberUsb::setExposure(bool exposure_on, bool exposure_auto, - float exposure_value) +float vpPylonGrabberUsb::setExposure(bool exposure_on, bool exposure_auto, float exposure_value) { connect(); if (exposure_on) - m_camera.ExposureMode.SetValue( - Basler_UsbCameraParams::ExposureMode_Timed); + m_camera.ExposureMode.SetValue(Basler_UsbCameraParams::ExposureMode_Timed); else - m_camera.ExposureMode.SetValue( - Basler_UsbCameraParams::ExposureMode_TriggerWidth); + m_camera.ExposureMode.SetValue(Basler_UsbCameraParams::ExposureMode_TriggerWidth); if (exposure_auto) - m_camera.ExposureAuto.SetValue( - Basler_UsbCameraParams::ExposureAuto_Continuous); + m_camera.ExposureAuto.SetValue(Basler_UsbCameraParams::ExposureAuto_Continuous); else m_camera.ExposureAuto.SetValue(Basler_UsbCameraParams::ExposureAuto_Off); @@ -456,8 +437,7 @@ float vpPylonGrabberUsb::setExposure(bool exposure_on, bool exposure_auto, m_camera.ExposureTime.SetValue(exposure_value * 1000); return m_camera.ExposureTime.GetValue() * 0.001; } else - throw vpException(vpException::notImplementedError, - "Don't know how to set exposure."); + throw vpException(vpException::notImplementedError, "Don't know how to set exposure."); } /*! @@ -483,8 +463,7 @@ float vpPylonGrabberUsb::setGamma(bool gamma_on, float gamma_value) m_camera.Gamma.SetValue(1); return m_camera.Gamma.GetValue(); } else - throw vpException(vpException::notImplementedError, - "Don't know how to set gamma."); + throw vpException(vpException::notImplementedError, "Don't know how to set gamma."); } /*! @@ -531,23 +510,19 @@ bool vpPylonGrabberUsb::setUserSetDefault(UserSetName user_set) switch (user_set) { case USERSET_DEFAULT: - m_camera.UserSetDefault.SetValue( - Basler_UsbCameraParams::UserSetDefault_Default); + m_camera.UserSetDefault.SetValue(Basler_UsbCameraParams::UserSetDefault_Default); return true; break; case USERSET_USERSET1: - m_camera.UserSetDefault.SetValue( - Basler_UsbCameraParams::UserSetDefault_UserSet1); + m_camera.UserSetDefault.SetValue(Basler_UsbCameraParams::UserSetDefault_UserSet1); return true; break; case USERSET_USERSET2: - m_camera.UserSetDefault.SetValue( - Basler_UsbCameraParams::UserSetDefault_UserSet2); + m_camera.UserSetDefault.SetValue(Basler_UsbCameraParams::UserSetDefault_UserSet2); return true; break; case USERSET_USERSET3: - m_camera.UserSetDefault.SetValue( - Basler_UsbCameraParams::UserSetDefault_UserSet3); + m_camera.UserSetDefault.SetValue(Basler_UsbCameraParams::UserSetDefault_UserSet3); return true; break; default: @@ -665,8 +640,7 @@ void vpPylonGrabberUsb::acquire(vpImage &I) Pylon::CGrabResultPtr grabResult; // Retrieve an image if (!m_camera.RetrieveResult(2000, grabResult)) { - throw(vpException(vpException::fatalError, - "Cannot retrieve image from camera with serial %s", + throw(vpException(vpException::fatalError, "Cannot retrieve image from camera with serial %s", getCameraSerial(m_index).c_str())); } @@ -679,8 +653,7 @@ void vpPylonGrabberUsb::acquire(vpImage &I) imageConvert.OutputPixelFormat = Pylon::PixelType_Mono8; imageConvert.OutputPaddingX = 0; // Create a converted image - imageConvert.Convert(I.bitmap, sizeof(unsigned char) * width * height, - (Pylon::IImage &)grabResult); + imageConvert.Convert(I.bitmap, sizeof(unsigned char) * width * height, (Pylon::IImage &)grabResult); } } @@ -696,8 +669,7 @@ void vpPylonGrabberUsb::acquire(vpImage &I) Pylon::CGrabResultPtr grabResult; // Retrieve an image if (!m_camera.RetrieveResult(2000, grabResult)) { - throw(vpException(vpException::fatalError, - "Cannot retrieve image from camera with serial %s", + throw(vpException(vpException::fatalError, "Cannot retrieve image from camera with serial %s", getCameraSerial(m_index).c_str())); } @@ -776,23 +748,19 @@ bool vpPylonGrabberUsb::selectUserSet(UserSetName user_set) switch (user_set) { case USERSET_DEFAULT: - m_camera.UserSetSelector.SetValue( - Basler_UsbCameraParams::UserSetSelector_Default); + m_camera.UserSetSelector.SetValue(Basler_UsbCameraParams::UserSetSelector_Default); return true; break; case USERSET_USERSET1: - m_camera.UserSetSelector.SetValue( - Basler_UsbCameraParams::UserSetSelector_UserSet1); + m_camera.UserSetSelector.SetValue(Basler_UsbCameraParams::UserSetSelector_UserSet1); return true; break; case USERSET_USERSET2: - m_camera.UserSetSelector.SetValue( - Basler_UsbCameraParams::UserSetSelector_UserSet2); + m_camera.UserSetSelector.SetValue(Basler_UsbCameraParams::UserSetSelector_UserSet2); return true; break; case USERSET_USERSET3: - m_camera.UserSetSelector.SetValue( - Basler_UsbCameraParams::UserSetSelector_UserSet3); + m_camera.UserSetSelector.SetValue(Basler_UsbCameraParams::UserSetSelector_UserSet3); return true; break; default: diff --git a/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.h b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.h index 202d64d2d3..c275bdcda5 100644 --- a/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.h +++ b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.h @@ -102,8 +102,7 @@ class VISP_EXPORT vpPylonGrabberUsb : public vpPylonGrabber float setBlackLevel(float blacklevel_value = 0); void setCameraIndex(unsigned int index); void setCameraSerial(const std::string &serial); - float setExposure(bool exposure_on, bool exposure_auto, - float exposure_value = 0); + float setExposure(bool exposure_on, bool exposure_auto, float exposure_value = 0); float setGain(bool gain_auto, float gain_value = 0); float setFrameRate(float frame_rate); float setGamma(bool gamma_on, float gamma_value = 1); @@ -120,8 +119,8 @@ class VISP_EXPORT vpPylonGrabberUsb : public vpPylonGrabber private: Pylon::CBaslerUsbInstantCamera m_camera; //!< Pointer to each camera unsigned int m_index; //!< Active camera index - unsigned int m_numCameras; //!< Number of connected USB cameras - bool m_connected; //!< true if camera connected + unsigned int m_numCameras; //!< Number of connected USB cameras + bool m_connected; //!< true if camera connected }; #endif // #ifdef VISP_HAVE_PYLON diff --git a/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp b/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp index 9a28a0e224..6b45f6fb70 100644 --- a/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp +++ b/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp @@ -147,13 +147,10 @@ int main() */ vpV4l2Grabber::vpV4l2Grabber() - : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), - ctl(NULL), 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) + : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), 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); @@ -206,14 +203,10 @@ vpV4l2Grabber::vpV4l2Grabber() */ vpV4l2Grabber::vpV4l2Grabber(bool verbose) - : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), - ctl(NULL), 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) + : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), 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); @@ -248,21 +241,18 @@ vpV4l2Grabber::vpV4l2Grabber(bool verbose) \code vpImage I; // Grey level image - vpV4l2Grabber g(1, 2); // Select input 1, and half full size resolution - images. g.open(I); // Open the grabber + 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), 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) + : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), 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); @@ -304,15 +294,11 @@ vpV4l2Grabber::vpV4l2Grabber(unsigned input, unsigned scale) \endcode */ -vpV4l2Grabber::vpV4l2Grabber(vpImage &I, unsigned input, - unsigned scale) - : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), - ctl(NULL), 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) +vpV4l2Grabber::vpV4l2Grabber(vpImage &I, unsigned input, unsigned scale) + : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), 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); @@ -357,15 +343,11 @@ vpV4l2Grabber::vpV4l2Grabber(vpImage &I, unsigned input, \endcode */ -vpV4l2Grabber::vpV4l2Grabber(vpImage &I, unsigned input, - unsigned scale) - : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), - ctl(NULL), 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) +vpV4l2Grabber::vpV4l2Grabber(vpImage &I, unsigned input, unsigned scale) + : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), 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); @@ -408,8 +390,7 @@ void vpV4l2Grabber::setScale(unsigned scale) close(); vpERROR_TRACE("Wrong scale %d, scale should be between 1 and 16", scale); - throw(vpFrameGrabberException(vpFrameGrabberException::settingError, - "Wrong scale")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Wrong scale")); } setWidth(640 / scale); @@ -431,8 +412,7 @@ void vpV4l2Grabber::open(vpImage &I) open(); if (v4l2_ioctl(fd, VIDIOC_S_INPUT, &m_input) == -1) { - std::cout << "Warning: cannot set input channel to " << m_input - << std::endl; + std::cout << "Warning: cannot set input channel to " << m_input << std::endl; } vpV4l2PixelFormatType req_pixelformat = getPixelFormat(); @@ -443,8 +423,7 @@ void vpV4l2Grabber::open(vpImage &I) startStreaming(); } catch (...) { if (m_verbose) { - std::cout << "Requested pixel format [" << req_pixelformat - << "] not compatible with camera" << std::endl; + std::cout << "Requested pixel format [" << req_pixelformat << "] not compatible with camera" << std::endl; std::cout << "Try to found a compatible pixel format..." << std::endl; } @@ -458,22 +437,18 @@ void vpV4l2Grabber::open(vpImage &I) setFormat(); startStreaming(); if (m_verbose) - std::cout << "This format [" << m_pixelformat - << "] is compatible with camera" << std::endl; + 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; + 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; + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, + "No pixel format compatible with the camera was found")); } } } @@ -498,8 +473,7 @@ void vpV4l2Grabber::open(vpImage &I) open(); if (v4l2_ioctl(fd, VIDIOC_S_INPUT, &m_input) == -1) { - std::cout << "Warning: cannot set input channel to " << m_input - << std::endl; + std::cout << "Warning: cannot set input channel to " << m_input << std::endl; } vpV4l2PixelFormatType req_pixelformat = getPixelFormat(); @@ -510,8 +484,7 @@ void vpV4l2Grabber::open(vpImage &I) startStreaming(); } catch (...) { if (m_verbose) { - std::cout << "Requested pixel format [" << m_pixelformat - << "] not compatible with camera" << std::endl; + std::cout << "Requested pixel format [" << m_pixelformat << "] not compatible with camera" << std::endl; std::cout << "Try to found a compatible pixel format..." << std::endl; } @@ -525,14 +498,12 @@ void vpV4l2Grabber::open(vpImage &I) setFormat(); startStreaming(); if (m_verbose) - std::cout << "This format [" << m_pixelformat - << "] is compatible with camera" << std::endl; + 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; + std::cout << "This format [" << m_pixelformat << "] is not compatible with camera" << std::endl; } } } @@ -597,8 +568,7 @@ void vpV4l2Grabber::acquire(vpImage &I, const vpRect &roi) \sa getField() */ -void vpV4l2Grabber::acquire(vpImage &I, - struct timeval ×tamp, const vpRect &roi) +void vpV4l2Grabber::acquire(vpImage &I, struct timeval ×tamp, const vpRect &roi) { if (init == false) { open(I); @@ -607,9 +577,7 @@ void vpV4l2Grabber::acquire(vpImage &I, if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "V4l2 frame grabber not initialized")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "V4l2 frame grabber not initialized")); } unsigned char *bitmap; @@ -628,46 +596,38 @@ void vpV4l2Grabber::acquire(vpImage &I, break; case V4L2_RGB24_FORMAT: // tested if (roi == vpRect()) - vpImageConvert::RGBToGrey((unsigned char *)bitmap, I.bitmap, - width * height); + vpImageConvert::RGBToGrey((unsigned char *)bitmap, I.bitmap, width * height); else { vpImage tmp(height, width); - vpImageConvert::RGBToGrey((unsigned char *)bitmap, tmp.bitmap, - width * height); + vpImageConvert::RGBToGrey((unsigned char *)bitmap, tmp.bitmap, width * height); vpImageTools::crop(tmp, roi, I); } break; case V4L2_RGB32_FORMAT: if (roi == vpRect()) - vpImageConvert::RGBaToGrey((unsigned char *)bitmap, I.bitmap, - width * height); + vpImageConvert::RGBaToGrey((unsigned char *)bitmap, I.bitmap, width * height); else { vpImage tmp(height, width); - vpImageConvert::RGBaToGrey((unsigned char *)bitmap, tmp.bitmap, - width * height); + vpImageConvert::RGBaToGrey((unsigned char *)bitmap, tmp.bitmap, width * height); vpImageTools::crop(tmp, roi, I); } break; case V4L2_BGR24_FORMAT: // tested if (roi == vpRect()) - vpImageConvert::BGRToGrey((unsigned char *)bitmap, I.bitmap, width, - height, false); + vpImageConvert::BGRToGrey((unsigned char *)bitmap, I.bitmap, width, height, false); else { vpImage tmp(height, width); - vpImageConvert::BGRToGrey((unsigned char *)bitmap, tmp.bitmap, width, - height, false); + vpImageConvert::BGRToGrey((unsigned char *)bitmap, tmp.bitmap, width, height, false); vpImageTools::crop(tmp, roi, I); } break; case V4L2_YUYV_FORMAT: // tested if (roi == vpRect()) - vpImageConvert::YUYVToGrey((unsigned char *)bitmap, I.bitmap, - width * height); + vpImageConvert::YUYVToGrey((unsigned char *)bitmap, I.bitmap, width * height); else { vpImage tmp(height, width); - vpImageConvert::YUYVToGrey((unsigned char *)bitmap, tmp.bitmap, - width * height); + vpImageConvert::YUYVToGrey((unsigned char *)bitmap, tmp.bitmap, width * height); vpImageTools::crop(tmp, roi, I); } break; @@ -734,8 +694,7 @@ void vpV4l2Grabber::acquire(vpImage &I, const vpRect &roi) \sa getField() */ -void vpV4l2Grabber::acquire(vpImage &I, struct timeval ×tamp, - const vpRect &roi) +void vpV4l2Grabber::acquire(vpImage &I, struct timeval ×tamp, const vpRect &roi) { if (init == false) { open(I); @@ -744,9 +703,7 @@ void vpV4l2Grabber::acquire(vpImage &I, struct timeval ×tamp, if (init == false) { close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::initializationError, - "V4l2 frame grabber not initialized")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "V4l2 frame grabber not initialized")); } unsigned char *bitmap; @@ -763,19 +720,16 @@ void vpV4l2Grabber::acquire(vpImage &I, struct timeval ×tamp, switch (m_pixelformat) { case V4L2_GREY_FORMAT: if (roi == vpRect()) - vpImageConvert::GreyToRGBa((unsigned char *)bitmap, - (unsigned char *)I.bitmap, width * height); + vpImageConvert::GreyToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width * height); else vpImageTools::crop(bitmap, width, height, roi, I); break; case V4L2_RGB24_FORMAT: // tested if (roi == vpRect()) - vpImageConvert::RGBToRGBa((unsigned char *)bitmap, - (unsigned char *)I.bitmap, width * height); + vpImageConvert::RGBToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width * height); else { vpImage tmp(height, width); - vpImageConvert::RGBToRGBa((unsigned char *)bitmap, - (unsigned char *)tmp.bitmap, width * height); + vpImageConvert::RGBToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width * height); vpImageTools::crop(tmp, roi, I); } break; @@ -787,9 +741,7 @@ void vpV4l2Grabber::acquire(vpImage &I, struct timeval ×tamp, I[height - 1][width - 1].A = 0; } else { for (unsigned int i = 0; i < I.getHeight(); i++) { - memcpy(I.bitmap, - bitmap + 1 + - (unsigned int)(roi.getTop() * width + roi.getLeft()), + memcpy(I.bitmap, bitmap + 1 + (unsigned int)(roi.getTop() * width + roi.getLeft()), I.getWidth() * sizeof(vpRGBa) - 1); I[i][I.getWidth() - 1].A = 0; } @@ -797,25 +749,19 @@ void vpV4l2Grabber::acquire(vpImage &I, struct timeval ×tamp, break; case V4L2_BGR24_FORMAT: // tested if (roi == vpRect()) - vpImageConvert::BGRToRGBa((unsigned char *)bitmap, - (unsigned char *)I.bitmap, width, height, - false); + vpImageConvert::BGRToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width, height, false); else { vpImage tmp(height, width); - vpImageConvert::BGRToRGBa((unsigned char *)bitmap, - (unsigned char *)tmp.bitmap, width, height, - false); + vpImageConvert::BGRToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width, height, false); vpImageTools::crop(tmp, roi, I); } break; case V4L2_YUYV_FORMAT: // tested if (roi == vpRect()) - vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, - (unsigned char *)I.bitmap, width, height); + vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width, height); else { vpImage tmp(height, width); - vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, - (unsigned char *)tmp.bitmap, width, height); + vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width, height); vpImageTools::crop(tmp, roi, I); } break; @@ -851,8 +797,7 @@ bool vpV4l2Grabber::getField() else { close(); - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "V4l2 returns a bad frame field")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "V4l2 returns a bad frame field")); return false; } } @@ -889,10 +834,7 @@ void vpV4l2Grabber::setFramerate(vpV4l2Grabber::vpV4l2FramerateType framerate) \sa setFramerate() */ -vpV4l2Grabber::vpV4l2FramerateType vpV4l2Grabber::getFramerate() -{ - return m_framerate; -} +vpV4l2Grabber::vpV4l2FramerateType vpV4l2Grabber::getFramerate() { return m_framerate; } /*! Close the video device. @@ -951,26 +893,20 @@ void vpV4l2Grabber::open() 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")); + 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")); + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Can't access to video device")); } if (inp != NULL) { @@ -1009,16 +945,14 @@ void vpV4l2Grabber::open() 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")); + 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, + 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"); @@ -1047,8 +981,7 @@ void vpV4l2Grabber::open() // 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); + fprintf(stdout, " Current acquisition framerate: %d fps\n", streamparm.parm.output.timeperframe.denominator); } } @@ -1084,8 +1017,7 @@ void vpV4l2Grabber::getCapabilities() if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) == -1) { close(); - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "Can't get video parameters")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't get video parameters")); } } @@ -1140,9 +1072,7 @@ void vpV4l2Grabber::setFormat() default: close(); - throw( - vpFrameGrabberException(vpFrameGrabberException::settingError, - "Bad format, probably do to a wrong scale")); + throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Bad format, probably do to a wrong scale")); } /* Get Video Format */ @@ -1153,8 +1083,7 @@ void vpV4l2Grabber::setFormat() if (v4l2_ioctl(fd, VIDIOC_G_FMT, &fmt_v4l2) == -1) { close(); - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "Can't get video format")); + 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; @@ -1178,8 +1107,7 @@ void vpV4l2Grabber::setFormat() default: close(); - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "Unrecognized frame format")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Unrecognized frame format")); } // height and width of the captured image or frame @@ -1190,13 +1118,11 @@ void vpV4l2Grabber::setFormat() // fmt_v4l2.fmt.pix.height); if (v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt_v4l2) == -1) { - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "Can't set video format")); + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Bad pixel format")); } /* Buggy driver paranoia. */ @@ -1216,10 +1142,8 @@ void vpV4l2Grabber::setFormat() "v4l2: new capture params (%ux%u, %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); + (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); } } /*! @@ -1249,11 +1173,9 @@ void vpV4l2Grabber::startStreaming() "%s does not support " "memory mapping\n", device); - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "Does not support memory mapping")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Does not support memory mapping")); } - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "Can't require video buffers")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't require video buffers")); } for (unsigned i = 0; i < reqbufs.count; i++) { @@ -1264,8 +1186,7 @@ void vpV4l2Grabber::startStreaming() 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")); + 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; @@ -1276,13 +1197,11 @@ void vpV4l2Grabber::startStreaming() // << " 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); + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't map memory")); } buf_me[i].refcount = 0; @@ -1306,8 +1225,7 @@ void vpV4l2Grabber::startStreaming() /* Set video stream capture on */ if (v4l2_ioctl(fd, VIDIOC_STREAMON, &fmt_v4l2.type) < 0) { - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "Can't start streaming")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't start streaming")); } streaming = true; @@ -1328,8 +1246,7 @@ void vpV4l2Grabber::stopStreaming() /* 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't stop streaming")); } /* free buffers */ for (unsigned int i = 0; i < reqbufs.count; i++) { @@ -1338,8 +1255,7 @@ void vpV4l2Grabber::stopStreaming() // vpTRACE("v4l2_munmap()"); if (-1 == v4l2_munmap(buf_me[i].data, buf_me[i].size)) { - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "Can't unmap memory")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't unmap memory")); } } queue = 0; @@ -1379,13 +1295,11 @@ unsigned char *vpV4l2Grabber::waiton(__u32 &index, struct timeval ×tamp) if (EINTR == errno) goto again; index = 0; - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "Can't access to the frame")); + 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")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't access to the frame: timeout")); return NULL; } @@ -1397,20 +1311,16 @@ unsigned char *vpV4l2Grabber::waiton(__u32 &index, struct timeval ×tamp) index = 0; switch (errno) { case EAGAIN: - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "VIDIOC_DQBUF: EAGAIN")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: EAGAIN")); break; case EINVAL: - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "VIDIOC_DQBUF: EINVAL")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: EINVAL")); break; case ENOMEM: - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "VIDIOC_DQBUF: ENOMEM")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: ENOMEM")); break; default: - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "VIDIOC_DQBUF")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF")); break; } return NULL; @@ -1449,8 +1359,7 @@ int vpV4l2Grabber::queueBuffer() 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 << "Normalement call ng_waiton_video_buf(buf_me+frame); --------\n"; } // std::cout << "frame: " << frame << std::endl; @@ -1460,20 +1369,16 @@ int vpV4l2Grabber::queueBuffer() else { switch (errno) { case EAGAIN: - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "VIDIOC_QBUF: EAGAIN")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: EAGAIN")); break; case EINVAL: - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "VIDIOC_QBUF: EINVAL")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: EINVAL")); break; case ENOMEM: - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "VIDIOC_QBUF: ENOMEM")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: ENOMEM")); break; default: - throw(vpFrameGrabberException(vpFrameGrabberException::otherError, - "VIDIOC_QBUF")); + throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF")); break; } } @@ -1527,10 +1432,8 @@ void vpV4l2Grabber::printBufInfo(struct v4l2_buffer buf) 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); + 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); } /*! diff --git a/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp b/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp index 3c08450b2b..2e7ef82d7f 100644 --- a/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp +++ b/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp @@ -36,8 +36,7 @@ * *****************************************************************************/ -#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) +#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) #include #include @@ -70,8 +69,7 @@ body messages. */ vpSickLDMRS::vpSickLDMRS() - : socket_fd(-1), body(NULL), vAngle(), time_offset(0), isFirstMeasure(true), - maxlen_body(104000) + : socket_fd(-1), body(NULL), vAngle(), time_offset(0), isFirstMeasure(true), maxlen_body(104000) { ip = "131.254.12.119"; port = 12002; @@ -142,8 +140,7 @@ bool vpSickLDMRS::setup() FD_SET(static_cast(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)); + fprintf(stderr, "Error connecting to server %d - %s\n", errno, strerror(errno)); return false; } else if (res > 0) { fprintf(stderr, "ok"); @@ -217,15 +214,13 @@ bool vpSickLDMRS::measure(vpLaserScan laserscan[4]) uintptr = (unsigned int *)(body + 6); unsigned int seconds = uintptr[1]; unsigned int fractional = uintptr[0]; - double startTimestamp = - seconds + fractional / 4294967296.; // 4294967296. = 2^32 + 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 + double endTimestamp = seconds + fractional / 4294967296.; // 4294967296. = 2^32 // compute the time offset to bring the measures in the Unix time reference if (isFirstMeasure) { diff --git a/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp b/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp index cd26353106..e7d306dd9d 100644 --- a/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp +++ b/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp @@ -51,9 +51,8 @@ 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), + : 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); @@ -79,17 +78,15 @@ void vpKinect::start(vpKinect::vpDMResolution res) //! 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; + 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.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; + 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); @@ -100,20 +97,16 @@ void vpKinect::start(vpKinect::vpDMResolution res) #if defined(VISP_HAVE_VIPER850_DATA) && defined(VISP_HAVE_XML2) vpXmlParserCamera cameraParser; - std::string cameraXmlFile = - std::string(VISP_VIPER850_DATA_PATH) + - std::string("/include/const_camera_Viper850.xml"); - cameraParser.parse(RGBcam, cameraXmlFile, "Generic-camera", - vpCameraParameters::perspectiveProjWithDistortion, width, + std::string cameraXmlFile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/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 + RGBcam.initPersProjWithDistortion(522.5431816996, 522.7191431808, 311.4001982614, 267.4283562142, 0.0477365207, + -0.0462326418); // new #endif this->startVideo(); @@ -163,11 +156,9 @@ void vpKinect::DepthCallback(void *depth, uint32_t /* timestamp */) 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 + 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; } } @@ -251,13 +242,10 @@ bool vpKinect::getRGB(vpImage &I_RGB) 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 &Irgb, - const vpImage &Idepth, - vpImage &IrgbWarped) +void vpKinect::warpRGBFrame(const vpImage &Irgb, const vpImage &Idepth, vpImage &IrgbWarped) { if ((Idepth.getHeight() != hd) || (Idepth.getWidth() != wd)) { - vpERROR_TRACE(1, - "Idepth image size does not match vpKinect DM resolution"); + vpERROR_TRACE(1, "Idepth image size does not match vpKinect DM resolution"); } else { if ((IrgbWarped.getHeight() != hd) || (IrgbWarped.getWidth() != wd)) IrgbWarped.resize(hd, wd); diff --git a/modules/sensor/src/rgb-depth/realsense/vpRealSense.cpp b/modules/sensor/src/rgb-depth/realsense/vpRealSense.cpp index 78983a06b5..754329a31c 100644 --- a/modules/sensor/src/rgb-depth/realsense/vpRealSense.cpp +++ b/modules/sensor/src/rgb-depth/realsense/vpRealSense.cpp @@ -50,9 +50,8 @@ * Default constructor. */ vpRealSense::vpRealSense() - : m_context(), m_device(NULL), m_num_devices(0), m_serial_no(), - m_intrinsics(), m_max_Z(8), m_enableStreams(), m_useStreamPresets(), - m_streamPresets(), m_streamParams(), m_invalidDepthValue(0.0f) + : m_context(), m_device(NULL), m_num_devices(0), m_serial_no(), m_intrinsics(), m_max_Z(8), m_enableStreams(), + m_useStreamPresets(), m_streamPresets(), m_streamParams(), m_invalidDepthValue(0.0f) { initStream(); } @@ -69,26 +68,22 @@ void vpRealSense::initStream() // Color stream m_useStreamPresets[rs::stream::color] = true; m_streamPresets[rs::stream::color] = rs::preset::best_quality; - m_streamParams[rs::stream::color] = - vpRsStreamParams(640, 480, rs::format::rgba8, 60); + m_streamParams[rs::stream::color] = vpRsStreamParams(640, 480, rs::format::rgba8, 60); // Depth stream m_useStreamPresets[rs::stream::depth] = true; m_streamPresets[rs::stream::depth] = rs::preset::best_quality; - m_streamParams[rs::stream::depth] = - vpRsStreamParams(640, 480, rs::format::z16, 60); + m_streamParams[rs::stream::depth] = vpRsStreamParams(640, 480, rs::format::z16, 60); // Infrared stream m_useStreamPresets[rs::stream::infrared] = true; m_streamPresets[rs::stream::infrared] = rs::preset::best_quality; - m_streamParams[rs::stream::infrared] = - vpRsStreamParams(640, 480, rs::format::y16, 60); + m_streamParams[rs::stream::infrared] = vpRsStreamParams(640, 480, rs::format::y16, 60); // Infrared stream 2 m_useStreamPresets[rs::stream::infrared2] = true; m_streamPresets[rs::stream::infrared2] = rs::preset::best_quality; - m_streamParams[rs::stream::infrared2] = - vpRsStreamParams(640, 480, rs::format::y16, 60); + m_streamParams[rs::stream::infrared2] = vpRsStreamParams(640, 480, rs::format::y16, 60); // Enable all streams m_enableStreams[rs::stream::color] = true; @@ -105,8 +100,7 @@ void vpRealSense::open() m_num_devices = m_context.get_device_count(); if (m_num_devices == 0) - throw vpException(vpException::fatalError, - "RealSense Camera - No device detected."); + throw vpException(vpException::fatalError, "RealSense Camera - No device detected."); std::vector detected_devices; rs::device *device; @@ -141,14 +135,12 @@ void vpRealSense::open() // the code will proceed. m_device = m_context.get_device(0); - std::cout << "RealSense Camera - Connecting to camera with Serial No: " - << m_device->get_serial() << std::endl; + std::cout << "RealSense Camera - Connecting to camera with Serial No: " << m_device->get_serial() << std::endl; // Enable only infrared2 stream if supported - m_enableStreams[rs::stream::infrared2] = - m_enableStreams[rs::stream::infrared2] - ? m_device->supports(rs::capabilities::infrared2) - : m_enableStreams[rs::stream::infrared2]; + m_enableStreams[rs::stream::infrared2] = m_enableStreams[rs::stream::infrared2] + ? m_device->supports(rs::capabilities::infrared2) + : m_enableStreams[rs::stream::infrared2]; if (m_device->is_streaming()) { m_device->stop(); @@ -164,55 +156,45 @@ void vpRealSense::open() if (m_enableStreams[rs::stream::color]) { if (m_useStreamPresets[rs::stream::color]) { - m_device->enable_stream(rs::stream::color, - m_streamPresets[rs::stream::color]); + m_device->enable_stream(rs::stream::color, m_streamPresets[rs::stream::color]); } else { - m_device->enable_stream( - rs::stream::color, m_streamParams[rs::stream::color].m_streamWidth, - m_streamParams[rs::stream::color].m_streamHeight, - m_streamParams[rs::stream::color].m_streamFormat, - m_streamParams[rs::stream::color].m_streamFramerate); + m_device->enable_stream(rs::stream::color, m_streamParams[rs::stream::color].m_streamWidth, + m_streamParams[rs::stream::color].m_streamHeight, + m_streamParams[rs::stream::color].m_streamFormat, + m_streamParams[rs::stream::color].m_streamFramerate); } } if (m_enableStreams[rs::stream::depth]) { if (m_useStreamPresets[rs::stream::depth]) { - m_device->enable_stream(rs::stream::depth, - m_streamPresets[rs::stream::depth]); + m_device->enable_stream(rs::stream::depth, m_streamPresets[rs::stream::depth]); } else { - m_device->enable_stream( - rs::stream::depth, m_streamParams[rs::stream::depth].m_streamWidth, - m_streamParams[rs::stream::depth].m_streamHeight, - m_streamParams[rs::stream::depth].m_streamFormat, - m_streamParams[rs::stream::depth].m_streamFramerate); + m_device->enable_stream(rs::stream::depth, m_streamParams[rs::stream::depth].m_streamWidth, + m_streamParams[rs::stream::depth].m_streamHeight, + m_streamParams[rs::stream::depth].m_streamFormat, + m_streamParams[rs::stream::depth].m_streamFramerate); } } if (m_enableStreams[rs::stream::infrared]) { if (m_useStreamPresets[rs::stream::infrared]) { - m_device->enable_stream(rs::stream::infrared, - m_streamPresets[rs::stream::infrared]); + m_device->enable_stream(rs::stream::infrared, m_streamPresets[rs::stream::infrared]); } else { - m_device->enable_stream( - rs::stream::infrared, - m_streamParams[rs::stream::infrared].m_streamWidth, - m_streamParams[rs::stream::infrared].m_streamHeight, - m_streamParams[rs::stream::infrared].m_streamFormat, - m_streamParams[rs::stream::infrared].m_streamFramerate); + m_device->enable_stream(rs::stream::infrared, m_streamParams[rs::stream::infrared].m_streamWidth, + m_streamParams[rs::stream::infrared].m_streamHeight, + m_streamParams[rs::stream::infrared].m_streamFormat, + m_streamParams[rs::stream::infrared].m_streamFramerate); } } if (m_enableStreams[rs::stream::infrared2]) { if (m_useStreamPresets[rs::stream::infrared2]) { - m_device->enable_stream(rs::stream::infrared2, - m_streamPresets[rs::stream::infrared2]); + m_device->enable_stream(rs::stream::infrared2, m_streamPresets[rs::stream::infrared2]); } else { - m_device->enable_stream( - rs::stream::infrared2, - m_streamParams[rs::stream::infrared2].m_streamWidth, - m_streamParams[rs::stream::infrared2].m_streamHeight, - m_streamParams[rs::stream::infrared2].m_streamFormat, - m_streamParams[rs::stream::infrared2].m_streamFramerate); + m_device->enable_stream(rs::stream::infrared2, m_streamParams[rs::stream::infrared2].m_streamWidth, + m_streamParams[rs::stream::infrared2].m_streamHeight, + m_streamParams[rs::stream::infrared2].m_streamFormat, + m_streamParams[rs::stream::infrared2].m_streamFramerate); } } @@ -229,8 +211,7 @@ void vpRealSense::open() // Add synthetic stream intrinsics if (m_enableStreams[rs::stream::color]) { - m_intrinsics[rs::stream::rectified_color] = - m_device->get_stream_intrinsics(rs::stream::rectified_color); + m_intrinsics[rs::stream::rectified_color] = m_device->get_stream_intrinsics(rs::stream::rectified_color); if (m_enableStreams[rs::stream::depth]) { m_intrinsics[rs::stream::color_aligned_to_depth] = @@ -238,19 +219,15 @@ void vpRealSense::open() m_intrinsics[rs::stream::depth_aligned_to_color] = m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_color); m_intrinsics[rs::stream::depth_aligned_to_rectified_color] = - m_device->get_stream_intrinsics( - rs::stream::depth_aligned_to_rectified_color); + m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_rectified_color); } } - if (m_enableStreams[rs::stream::depth] && - m_enableStreams[rs::stream::infrared2]) { + if (m_enableStreams[rs::stream::depth] && m_enableStreams[rs::stream::infrared2]) { m_intrinsics[rs::stream::depth_aligned_to_infrared2] = - m_device->get_stream_intrinsics( - rs::stream::depth_aligned_to_infrared2); + m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_infrared2); m_intrinsics[rs::stream::infrared2_aligned_to_depth] = - m_device->get_stream_intrinsics( - rs::stream::infrared2_aligned_to_depth); + m_device->get_stream_intrinsics(rs::stream::infrared2_aligned_to_depth); } // Start device @@ -306,9 +283,8 @@ void vpRealSense::setDeviceBySerialNumber(const std::string &serial_no) \sa getIntrinsics() */ -vpCameraParameters vpRealSense::getCameraParameters( - const rs::stream &stream, - vpCameraParameters::vpCameraParametersProjType type) const +vpCameraParameters vpRealSense::getCameraParameters(const rs::stream &stream, + vpCameraParameters::vpCameraParametersProjType type) const { auto intrinsics = this->getIntrinsics(stream); @@ -335,9 +311,8 @@ vpCameraParameters vpRealSense::getCameraParameters( rs::intrinsics vpRealSense::getIntrinsics(const rs::stream &stream) const { if (!m_device) { - throw vpException(vpException::fatalError, - "RealSense Camera - device handler is null. Cannot " - "retrieve intrinsics. Exiting!"); + throw vpException(vpException::fatalError, "RealSense Camera - device handler is null. Cannot " + "retrieve intrinsics. Exiting!"); } if (!m_device->is_stream_enabled(stream)) { throw vpException(vpException::fatalError, @@ -346,8 +321,7 @@ rs::intrinsics vpRealSense::getIntrinsics(const rs::stream &stream) const (int)stream); } - std::map::const_iterator it_intrin = - m_intrinsics.find(stream); + std::map::const_iterator it_intrin = m_intrinsics.find(stream); if (it_intrin != m_intrinsics.end()) { return it_intrin->second; } @@ -362,13 +336,11 @@ rs::intrinsics vpRealSense::getIntrinsics(const rs::stream &stream) const \sa getTransformation() */ -rs::extrinsics vpRealSense::getExtrinsics(const rs::stream &from, - const rs::stream &to) const +rs::extrinsics vpRealSense::getExtrinsics(const rs::stream &from, const rs::stream &to) const { if (!m_device) { - throw vpException(vpException::fatalError, - "RealSense Camera - device handler is null. Cannot " - "retrieve extrinsics. Exiting!"); + throw vpException(vpException::fatalError, "RealSense Camera - device handler is null. Cannot " + "retrieve extrinsics. Exiting!"); } if (!m_device->is_stream_enabled(from)) { throw vpException(vpException::fatalError, @@ -392,8 +364,7 @@ rs::extrinsics vpRealSense::getExtrinsics(const rs::stream &from, \sa getExtrinsics() */ -vpHomogeneousMatrix vpRealSense::getTransformation(const rs::stream &from, - const rs::stream &to) const +vpHomogeneousMatrix vpRealSense::getTransformation(const rs::stream &from, const rs::stream &to) const { rs::extrinsics extrinsics = this->getExtrinsics(from, to); vpTranslationVector t; @@ -414,8 +385,7 @@ vpHomogeneousMatrix vpRealSense::getTransformation(const rs::stream &from, void vpRealSense::acquire(vpImage &grey) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -434,12 +404,10 @@ void vpRealSense::acquire(vpImage &grey) column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of a point. */ -void vpRealSense::acquire(vpImage &grey, - std::vector &pointcloud) +void vpRealSense::acquire(vpImage &grey, std::vector &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -451,8 +419,7 @@ void vpRealSense::acquire(vpImage &grey, vp_rs_get_grey_impl(m_device, m_intrinsics, grey); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -464,8 +431,7 @@ void vpRealSense::acquire(vpImage &grey, void vpRealSense::acquire(std::vector &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -474,8 +440,7 @@ void vpRealSense::acquire(std::vector &pointcloud) m_device->wait_for_frames(); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -485,8 +450,7 @@ void vpRealSense::acquire(std::vector &pointcloud) void vpRealSense::acquire(vpImage &color) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -507,13 +471,11 @@ void vpRealSense::acquire(vpImage &color) column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of a point. */ -void vpRealSense::acquire(vpImage &color, vpImage &infrared, - vpImage &depth, +void vpRealSense::acquire(vpImage &color, vpImage &infrared, vpImage &depth, std::vector &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -525,15 +487,13 @@ void vpRealSense::acquire(vpImage &color, vpImage &infrared, vp_rs_get_color_impl(m_device, m_intrinsics, color); // Retrieve infrared image - vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, - infrared); + vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared); // Retrieve depth image vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -545,14 +505,11 @@ void vpRealSense::acquire(vpImage &color, vpImage &infrared, column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of a point. */ -void vpRealSense::acquire(vpImage &grey, - vpImage &infrared, - vpImage &depth, +void vpRealSense::acquire(vpImage &grey, vpImage &infrared, vpImage &depth, std::vector &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -564,15 +521,13 @@ void vpRealSense::acquire(vpImage &grey, vp_rs_get_grey_impl(m_device, m_intrinsics, grey); // Retrieve infrared image - vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, - infrared); + vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared); // Retrieve depth image vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -582,12 +537,10 @@ void vpRealSense::acquire(vpImage &grey, column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of a point. */ -void vpRealSense::acquire(vpImage &color, - std::vector &pointcloud) +void vpRealSense::acquire(vpImage &color, std::vector &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -599,8 +552,7 @@ void vpRealSense::acquire(vpImage &color, vp_rs_get_color_impl(m_device, m_intrinsics, color); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -620,16 +572,14 @@ void vpRealSense::acquire(vpImage &color, stream_infrared2 : Type of infrared2 stream (e.g. rs::stream::infrared2, rs::stream::infrared2_aligned_to_depth) if supported by the camera. */ -void vpRealSense::acquire( - unsigned char *const data_image, unsigned char *const data_depth, - std::vector *const data_pointCloud, - unsigned char *const data_infrared, unsigned char *const data_infrared2, - const rs::stream &stream_color, const rs::stream &stream_depth, - const rs::stream &stream_infrared, const rs::stream &stream_infrared2) +void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth, + std::vector *const data_pointCloud, unsigned char *const data_infrared, + unsigned char *const data_infrared2, const rs::stream &stream_color, + const rs::stream &stream_depth, const rs::stream &stream_infrared, + const rs::stream &stream_infrared2) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { @@ -639,31 +589,23 @@ void vpRealSense::acquire( m_device->wait_for_frames(); if (data_image != NULL) { - vp_rs_get_native_frame_data_impl( - m_device, m_intrinsics, rs::stream::color, data_image, stream_color); + vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color); } if (data_depth != NULL) { - vp_rs_get_native_frame_data_impl( - m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth); + vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth); if (data_pointCloud != NULL) { - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, - *data_pointCloud, m_invalidDepthValue, - stream_depth); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth); } } if (data_infrared != NULL) { - vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, - rs::stream::infrared, data_infrared, - stream_infrared); + vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared); } if (data_infrared2 != NULL) { - vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, - rs::stream::infrared2, data_infrared2, - stream_infrared2); + vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2); } } @@ -672,8 +614,7 @@ void vpRealSense::acquire( \param stream : Stream type (color / depth / infrared). \param preset : Preset to use. */ -void vpRealSense::setStreamSettings(const rs::stream &stream, - const rs::preset &preset) +void vpRealSense::setStreamSettings(const rs::stream &stream, const rs::preset &preset) { m_useStreamPresets[stream] = true; m_streamPresets[stream] = preset; @@ -698,8 +639,7 @@ int main() rs.setEnableStream(rs::stream::color, true); rs.setEnableStream(rs::stream::depth, false); rs.setEnableStream(rs::stream::infrared, false); - rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(1920, -1080, rs::format::rgba8, 30)); + rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(1920, 1080, rs::format::rgba8, 30)); rs.open(); @@ -707,8 +647,7 @@ int main() } \endcode */ -void vpRealSense::setStreamSettings(const rs::stream &stream, - const vpRsStreamParams ¶ms) +void vpRealSense::setStreamSettings(const rs::stream &stream, const vpRsStreamParams ¶ms) { m_useStreamPresets[stream] = false; m_streamParams[stream] = params; @@ -729,8 +668,7 @@ int main() rs.setEnableStream(rs::stream::color, true); rs.setEnableStream(rs::stream::depth, false); rs.setEnableStream(rs::stream::infrared, false); - rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(1920, -1080, rs::format::rgba8, 30)); + rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(1920, 1080, rs::format::rgba8, 30)); rs.open(); @@ -738,10 +676,7 @@ int main() } \endcode */ -void vpRealSense::setEnableStream(const rs::stream &stream, const bool status) -{ - m_enableStreams[stream] = status; -} +void vpRealSense::setEnableStream(const rs::stream &stream, const bool status) { m_enableStreams[stream] = status; } #ifdef VISP_HAVE_PCL /*! @@ -751,8 +686,7 @@ void vpRealSense::setEnableStream(const rs::stream &stream, const bool status) void vpRealSense::acquire(pcl::PointCloud::Ptr &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -761,8 +695,7 @@ void vpRealSense::acquire(pcl::PointCloud::Ptr &pointcloud) m_device->wait_for_frames(); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -772,8 +705,7 @@ void vpRealSense::acquire(pcl::PointCloud::Ptr &pointcloud) void vpRealSense::acquire(pcl::PointCloud::Ptr &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -782,8 +714,7 @@ void vpRealSense::acquire(pcl::PointCloud::Ptr &pointcloud) m_device->wait_for_frames(); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -791,12 +722,10 @@ void vpRealSense::acquire(pcl::PointCloud::Ptr &pointcloud) \param grey : Grey level image. \param pointcloud : Point cloud data information. */ -void vpRealSense::acquire(vpImage &grey, - pcl::PointCloud::Ptr &pointcloud) +void vpRealSense::acquire(vpImage &grey, pcl::PointCloud::Ptr &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -808,8 +737,7 @@ void vpRealSense::acquire(vpImage &grey, vp_rs_get_grey_impl(m_device, m_intrinsics, grey); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -817,12 +745,10 @@ void vpRealSense::acquire(vpImage &grey, \param color : Color image. \param pointcloud : Point cloud data information. */ -void vpRealSense::acquire(vpImage &color, - pcl::PointCloud::Ptr &pointcloud) +void vpRealSense::acquire(vpImage &color, pcl::PointCloud::Ptr &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -834,8 +760,7 @@ void vpRealSense::acquire(vpImage &color, vp_rs_get_color_impl(m_device, m_intrinsics, color); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -845,13 +770,11 @@ void vpRealSense::acquire(vpImage &color, \param depth : Depth image. \param pointcloud : Point cloud data information. */ -void vpRealSense::acquire(vpImage &color, vpImage &infrared, - vpImage &depth, +void vpRealSense::acquire(vpImage &color, vpImage &infrared, vpImage &depth, pcl::PointCloud::Ptr &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -863,15 +786,13 @@ void vpRealSense::acquire(vpImage &color, vpImage &infrared, vp_rs_get_color_impl(m_device, m_intrinsics, color); // Retrieve infrared image - vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, - infrared); + vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared); // Retrieve depth image vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -881,14 +802,11 @@ void vpRealSense::acquire(vpImage &color, vpImage &infrared, \param depth : Depth image. \param pointcloud : Point cloud data information. */ -void vpRealSense::acquire(vpImage &grey, - vpImage &infrared, - vpImage &depth, +void vpRealSense::acquire(vpImage &grey, vpImage &infrared, vpImage &depth, pcl::PointCloud::Ptr &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -900,15 +818,13 @@ void vpRealSense::acquire(vpImage &grey, vp_rs_get_grey_impl(m_device, m_intrinsics, grey); // Retrieve infrared image - vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, - infrared); + vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared); // Retrieve depth image vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -918,13 +834,11 @@ void vpRealSense::acquire(vpImage &grey, \param depth : Depth image. \param pointcloud : Point cloud data with texture information. */ -void vpRealSense::acquire(vpImage &color, vpImage &infrared, - vpImage &depth, +void vpRealSense::acquire(vpImage &color, vpImage &infrared, vpImage &depth, pcl::PointCloud::Ptr &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -936,15 +850,13 @@ void vpRealSense::acquire(vpImage &color, vpImage &infrared, vp_rs_get_color_impl(m_device, m_intrinsics, color); // Retrieve infrared image - vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, - infrared); + vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared); // Retrieve depth image vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -954,14 +866,11 @@ void vpRealSense::acquire(vpImage &color, vpImage &infrared, \param depth : Depth image. \param pointcloud : Point cloud data with texture information. */ -void vpRealSense::acquire(vpImage &grey, - vpImage &infrared, - vpImage &depth, +void vpRealSense::acquire(vpImage &grey, vpImage &infrared, vpImage &depth, pcl::PointCloud::Ptr &pointcloud) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { open(); @@ -973,15 +882,13 @@ void vpRealSense::acquire(vpImage &grey, vp_rs_get_grey_impl(m_device, m_intrinsics, grey); // Retrieve infrared image - vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, - infrared); + vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared); // Retrieve depth image vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth); // Retrieve point cloud - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue); } /*! @@ -1003,17 +910,15 @@ void vpRealSense::acquire(vpImage &grey, stream_infrared2 : Type of infrared2 stream (e.g. rs::stream::infrared2, rs::stream::infrared2_aligned_to_depth) if supported by the camera. */ -void vpRealSense::acquire( - unsigned char *const data_image, unsigned char *const data_depth, - std::vector *const data_pointCloud, - pcl::PointCloud::Ptr &pointcloud, - unsigned char *const data_infrared, unsigned char *const data_infrared2, - const rs::stream &stream_color, const rs::stream &stream_depth, - const rs::stream &stream_infrared, const rs::stream &stream_infrared2) +void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth, + std::vector *const data_pointCloud, + pcl::PointCloud::Ptr &pointcloud, unsigned char *const data_infrared, + unsigned char *const data_infrared2, const rs::stream &stream_color, + const rs::stream &stream_depth, const rs::stream &stream_infrared, + const rs::stream &stream_infrared2) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { @@ -1023,36 +928,27 @@ void vpRealSense::acquire( m_device->wait_for_frames(); if (data_image != NULL) { - vp_rs_get_native_frame_data_impl( - m_device, m_intrinsics, rs::stream::color, data_image, stream_color); + vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color); } if (data_depth != NULL) { - vp_rs_get_native_frame_data_impl( - m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth); + vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth); } if (data_pointCloud != NULL) { - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, - *data_pointCloud, m_invalidDepthValue, - stream_depth); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth); } if (pointcloud != NULL) { - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue, stream_depth); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_depth); } if (data_infrared != NULL) { - vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, - rs::stream::infrared, data_infrared, - stream_infrared); + vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared); } if (data_infrared2 != NULL) { - vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, - rs::stream::infrared2, data_infrared2, - stream_infrared2); + vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2); } } @@ -1074,17 +970,15 @@ void vpRealSense::acquire( stream_infrared2 : Type of infrared2 stream (e.g. rs::stream::infrared2, rs::stream::infrared2_aligned_to_depth) if supported by the camera. */ -void vpRealSense::acquire( - unsigned char *const data_image, unsigned char *const data_depth, - std::vector *const data_pointCloud, - pcl::PointCloud::Ptr &pointcloud, - unsigned char *const data_infrared, unsigned char *const data_infrared2, - const rs::stream &stream_color, const rs::stream &stream_depth, - const rs::stream &stream_infrared, const rs::stream &stream_infrared2) +void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth, + std::vector *const data_pointCloud, + pcl::PointCloud::Ptr &pointcloud, unsigned char *const data_infrared, + unsigned char *const data_infrared2, const rs::stream &stream_color, + const rs::stream &stream_depth, const rs::stream &stream_infrared, + const rs::stream &stream_infrared2) { if (m_device == NULL) { - throw vpException(vpException::fatalError, - "RealSense Camera - Device not opened!"); + throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!"); } if (!m_device->is_streaming()) { @@ -1094,37 +988,28 @@ void vpRealSense::acquire( m_device->wait_for_frames(); if (data_image != NULL) { - vp_rs_get_native_frame_data_impl( - m_device, m_intrinsics, rs::stream::color, data_image, stream_color); + vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color); } if (data_depth != NULL) { - vp_rs_get_native_frame_data_impl( - m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth); + vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth); } if (data_pointCloud != NULL) { - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, - *data_pointCloud, m_invalidDepthValue, - stream_depth); + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth); } if (pointcloud != NULL) { - vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, - m_invalidDepthValue, stream_color, + vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_color, stream_depth); } if (data_infrared != NULL) { - vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, - rs::stream::infrared, data_infrared, - stream_infrared); + vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared); } if (data_infrared2 != NULL) { - vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, - rs::stream::infrared2, data_infrared2, - stream_infrared2); + vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2); } } #endif @@ -1158,10 +1043,8 @@ std::ostream &operator<<(std::ostream &os, const vpRealSense &rs) if (!rs.getHandler()->is_stream_enabled(stream)) continue; auto intrin = rs.getHandler()->get_stream_intrinsics(stream); - std::cout << "Capturing " << stream << " at " << intrin.width << " x " - << intrin.height; - std::cout << std::setprecision(1) << std::fixed - << ", fov = " << intrin.hfov() << " x " << intrin.vfov() + std::cout << "Capturing " << stream << " at " << intrin.width << " x " << intrin.height; + std::cout << std::setprecision(1) << std::fixed << ", fov = " << intrin.hfov() << " x " << intrin.vfov() << ", distortion = " << intrin.model() << std::endl; } std::cout.precision(ss); diff --git a/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp b/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp index 32149d871a..ccafcd7ec0 100644 --- a/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp +++ b/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp @@ -48,9 +48,8 @@ * Default constructor. */ vpRealSense2::vpRealSense2() - : m_colorIntrinsics(), m_depth2ColorExtrinsics(), m_depthIntrinsics(), - m_depthScale(0.0f), m_invalidDepthValue(0.0f), m_max_Z(8.0f), m_pipe(), - m_pipelineProfile(), m_pointcloud(), m_points() + : m_colorIntrinsics(), m_depth2ColorExtrinsics(), m_depthIntrinsics(), m_depthScale(0.0f), m_invalidDepthValue(0.0f), + m_max_Z(8.0f), m_pipe(), m_pipelineProfile(), m_pointcloud(), m_points() { } @@ -90,10 +89,8 @@ void vpRealSense2::acquire(vpImage &color) \param data_infrared : Infrared image buffer or NULL if not wanted. \param align_to : Align to a reference stream or NULL if not wanted. */ -void vpRealSense2::acquire(unsigned char *const data_image, - unsigned char *const data_depth, - std::vector *const data_pointCloud, - unsigned char *const data_infrared, +void vpRealSense2::acquire(unsigned char *const data_image, unsigned char *const data_depth, + std::vector *const data_pointCloud, unsigned char *const data_infrared, rs2::align *const align_to) { auto data = m_pipe.wait_for_frames(); @@ -131,11 +128,9 @@ void vpRealSense2::acquire(unsigned char *const data_image, image buffer or NULL if not wanted. \param align_to : Align to a reference stream or NULL if not wanted. */ -void vpRealSense2::acquire(unsigned char *const data_image, - unsigned char *const data_depth, +void vpRealSense2::acquire(unsigned char *const data_image, unsigned char *const data_depth, std::vector *const data_pointCloud, - pcl::PointCloud::Ptr &pointcloud, - unsigned char *const data_infrared, + pcl::PointCloud::Ptr &pointcloud, unsigned char *const data_infrared, rs2::align *const align_to) { auto data = m_pipe.wait_for_frames(); @@ -175,11 +170,9 @@ void vpRealSense2::acquire(unsigned char *const data_image, or NULL if not wanted. \param align_to : Align to a reference stream or NULL if not wanted. */ -void vpRealSense2::acquire(unsigned char *const data_image, - unsigned char *const data_depth, +void vpRealSense2::acquire(unsigned char *const data_image, unsigned char *const data_depth, std::vector *const data_pointCloud, - pcl::PointCloud::Ptr &pointcloud, - unsigned char *const data_infrared, + pcl::PointCloud::Ptr &pointcloud, unsigned char *const data_infrared, rs2::align *const align_to) { auto data = m_pipe.wait_for_frames(); @@ -231,12 +224,10 @@ void vpRealSense2::close() { m_pipe.stop(); } \sa getIntrinsics() */ -vpCameraParameters vpRealSense2::getCameraParameters( - const rs2_stream &stream, - vpCameraParameters::vpCameraParametersProjType type) const +vpCameraParameters vpRealSense2::getCameraParameters(const rs2_stream &stream, + vpCameraParameters::vpCameraParametersProjType type) const { - auto rs_stream = - m_pipelineProfile.get_stream(stream).as(); + auto rs_stream = m_pipelineProfile.get_stream(stream).as(); auto intrinsics = rs_stream.get_intrinsics(); vpCameraParameters cam; @@ -264,13 +255,11 @@ vpCameraParameters vpRealSense2::getCameraParameters( */ rs2_intrinsics vpRealSense2::getIntrinsics(const rs2_stream &stream) const { - auto vsp = - m_pipelineProfile.get_stream(stream).as(); + auto vsp = m_pipelineProfile.get_stream(stream).as(); return vsp.get_intrinsics(); } -void vpRealSense2::getColorFrame(const rs2::frame &frame, - vpImage &color) +void vpRealSense2::getColorFrame(const rs2::frame &frame, vpImage &color) { auto vf = frame.as(); const unsigned int width = (unsigned int)vf.get_width(); @@ -278,22 +267,17 @@ void vpRealSense2::getColorFrame(const rs2::frame &frame, color.resize(height, width); if (frame.get_profile().format() == RS2_FORMAT_RGB8) { - vpImageConvert::RGBToRGBa((unsigned char *)frame.get_data(), - (unsigned char *)color.bitmap, width, height); + vpImageConvert::RGBToRGBa((unsigned char *)frame.get_data(), (unsigned char *)color.bitmap, width, height); } else if (frame.get_profile().format() == RS2_FORMAT_RGBA8) { - memcpy((unsigned char *)color.bitmap, (unsigned char *)frame.get_data(), - width * height * sizeof(vpRGBa)); + memcpy((unsigned char *)color.bitmap, (unsigned char *)frame.get_data(), width * height * sizeof(vpRGBa)); } else if (frame.get_profile().format() == RS2_FORMAT_BGR8) { - vpImageConvert::BGRToRGBa((unsigned char *)frame.get_data(), - (unsigned char *)color.bitmap, width, height); + vpImageConvert::BGRToRGBa((unsigned char *)frame.get_data(), (unsigned char *)color.bitmap, width, height); } else { - throw vpException(vpException::fatalError, - "RealSense Camera - color stream not supported!"); + throw vpException(vpException::fatalError, "RealSense Camera - color stream not supported!"); } } -void vpRealSense2::getGreyFrame(const rs2::frame &frame, - vpImage &grey) +void vpRealSense2::getGreyFrame(const rs2::frame &frame, vpImage &grey) { auto vf = frame.as(); const unsigned int width = (unsigned int)vf.get_width(); @@ -301,22 +285,17 @@ void vpRealSense2::getGreyFrame(const rs2::frame &frame, grey.resize(height, width); if (frame.get_profile().format() == RS2_FORMAT_RGB8) { - vpImageConvert::RGBToGrey((unsigned char *)frame.get_data(), - (unsigned char *)grey.bitmap, width, height); + vpImageConvert::RGBToGrey((unsigned char *)frame.get_data(), (unsigned char *)grey.bitmap, width, height); } else if (frame.get_profile().format() == RS2_FORMAT_RGBA8) { - vpImageConvert::RGBaToGrey((unsigned char *)frame.get_data(), - (unsigned char *)grey.bitmap, width * height); + vpImageConvert::RGBaToGrey((unsigned char *)frame.get_data(), (unsigned char *)grey.bitmap, width * height); } else if (frame.get_profile().format() == RS2_FORMAT_BGR8) { - vpImageConvert::BGRToGrey((unsigned char *)frame.get_data(), - (unsigned char *)grey.bitmap, width, height); + vpImageConvert::BGRToGrey((unsigned char *)frame.get_data(), (unsigned char *)grey.bitmap, width, height); } else { - throw vpException(vpException::fatalError, - "RealSense Camera - color stream not supported!"); + throw vpException(vpException::fatalError, "RealSense Camera - color stream not supported!"); } } -void vpRealSense2::getNativeFrameData(const rs2::frame &frame, - unsigned char *const data) +void vpRealSense2::getNativeFrameData(const rs2::frame &frame, unsigned char *const data) { auto vf = frame.as(); int size = vf.get_width() * vf.get_height(); @@ -346,16 +325,14 @@ void vpRealSense2::getNativeFrameData(const rs2::frame &frame, } } -void vpRealSense2::getPointcloud(const rs2::depth_frame &depth_frame, - std::vector &pointcloud) +void vpRealSense2::getPointcloud(const rs2::depth_frame &depth_frame, std::vector &pointcloud) { auto vf = depth_frame.as(); const int width = vf.get_width(); const int height = vf.get_height(); pointcloud.resize((size_t)(width * height)); - const uint16_t *p_depth_frame = - reinterpret_cast(depth_frame.get_data()); + const uint16_t *p_depth_frame = reinterpret_cast(depth_frame.get_data()); #pragma omp parallel for schedule(dynamic) for (int i = 0; i < height; i++) { @@ -367,8 +344,7 @@ void vpRealSense2::getPointcloud(const rs2::depth_frame &depth_frame, float points[3]; const float pixel[] = {(float)j, (float)i}; - rs2_deproject_pixel_to_point(points, &m_depthIntrinsics, pixel, - pixels_distance); + rs2_deproject_pixel_to_point(points, &m_depthIntrinsics, pixel, pixels_distance); if (pixels_distance <= 0 || pixels_distance > m_max_Z) points[0] = points[1] = points[2] = m_invalidDepthValue; @@ -384,9 +360,7 @@ void vpRealSense2::getPointcloud(const rs2::depth_frame &depth_frame, } #ifdef VISP_HAVE_PCL -void vpRealSense2::getPointcloud( - const rs2::depth_frame &depth_frame, - pcl::PointCloud::Ptr &pointcloud) +void vpRealSense2::getPointcloud(const rs2::depth_frame &depth_frame, pcl::PointCloud::Ptr &pointcloud) { auto vf = depth_frame.as(); const int width = vf.get_width(); @@ -396,8 +370,7 @@ void vpRealSense2::getPointcloud( pointcloud->resize((size_t)(width * height)); #if MANUAL_POINTCLOUD // faster when tested - const uint16_t *p_depth_frame = - reinterpret_cast(depth_frame.get_data()); + const uint16_t *p_depth_frame = reinterpret_cast(depth_frame.get_data()); #pragma omp parallel for schedule(dynamic) for (int i = 0; i < height; i++) { @@ -409,8 +382,7 @@ void vpRealSense2::getPointcloud( float points[3]; const float pixel[] = {(float)j, (float)i}; - rs2_deproject_pixel_to_point(points, &m_depthIntrinsics, pixel, - pixels_distance); + rs2_deproject_pixel_to_point(points, &m_depthIntrinsics, pixel, pixels_distance); if (pixels_distance <= 0 || pixels_distance > m_max_Z) points[0] = points[1] = points[2] = m_invalidDepthValue; @@ -438,9 +410,8 @@ void vpRealSense2::getPointcloud( #endif } -void vpRealSense2::getPointcloud( - const rs2::depth_frame &depth_frame, const rs2::frame &color_frame, - pcl::PointCloud::Ptr &pointcloud) +void vpRealSense2::getPointcloud(const rs2::depth_frame &depth_frame, const rs2::frame &color_frame, + pcl::PointCloud::Ptr &pointcloud) { auto vf = depth_frame.as(); const int width = vf.get_width(); @@ -449,20 +420,13 @@ void vpRealSense2::getPointcloud( pointcloud->height = (uint32_t)height; pointcloud->resize((size_t)(width * height)); - const uint16_t *p_depth_frame = - reinterpret_cast(depth_frame.get_data()); + const uint16_t *p_depth_frame = reinterpret_cast(depth_frame.get_data()); const int color_width = m_colorIntrinsics.width; const int color_height = m_colorIntrinsics.height; - auto color_format = - color_frame.as().get_profile().format(); - bool swap_rgb = - color_format == RS2_FORMAT_BGR8 || color_format == RS2_FORMAT_BGRA8; - unsigned int nb_color_pixel = - (color_format == RS2_FORMAT_RGB8 || color_format == RS2_FORMAT_BGR8) - ? 3 - : 4; - const unsigned char *p_color_frame = - reinterpret_cast(color_frame.get_data()); + auto color_format = color_frame.as().get_profile().format(); + bool swap_rgb = color_format == RS2_FORMAT_BGR8 || color_format == RS2_FORMAT_BGRA8; + unsigned int nb_color_pixel = (color_format == RS2_FORMAT_RGB8 || color_format == RS2_FORMAT_BGR8) ? 3 : 4; + const unsigned char *p_color_frame = reinterpret_cast(color_frame.get_data()); #pragma omp parallel for schedule(dynamic) for (int i = 0; i < height; i++) { @@ -474,37 +438,29 @@ void vpRealSense2::getPointcloud( float depth_point[3]; const float pixel[] = {(float)j, (float)i}; - rs2_deproject_pixel_to_point(depth_point, &m_depthIntrinsics, pixel, - pixels_distance); + rs2_deproject_pixel_to_point(depth_point, &m_depthIntrinsics, pixel, pixels_distance); if (pixels_distance <= 0 || pixels_distance > m_max_Z) - depth_point[0] = depth_point[1] = depth_point[2] = - m_invalidDepthValue; + depth_point[0] = depth_point[1] = depth_point[2] = m_invalidDepthValue; pointcloud->points[(size_t)depth_pixel_index].x = depth_point[0]; pointcloud->points[(size_t)depth_pixel_index].y = depth_point[1]; pointcloud->points[(size_t)depth_pixel_index].z = depth_point[2]; float color_point[3]; - rs2_transform_point_to_point(color_point, &m_depth2ColorExtrinsics, - depth_point); + rs2_transform_point_to_point(color_point, &m_depth2ColorExtrinsics, depth_point); float color_pixel[2]; - rs2_project_point_to_pixel(color_pixel, &m_colorIntrinsics, - color_point); + rs2_project_point_to_pixel(color_pixel, &m_colorIntrinsics, color_point); - if (color_pixel[1] < 0 || color_pixel[1] >= color_height || - color_pixel[0] < 0 || color_pixel[0] >= color_width) { + if (color_pixel[1] < 0 || color_pixel[1] >= color_height || color_pixel[0] < 0 || color_pixel[0] >= color_width) { // For out of bounds color data, default to a shade of blue in order to // visually distinguish holes. This color value is same as the librealsense // out of bounds color value. #if PCL_VERSION_COMPARE(<, 1, 1, 0) unsigned int r = 96, g = 157, b = 198; - uint32_t rgb = - (static_cast(r) << 16 | static_cast(g) << 8 | - static_cast(b)); + uint32_t rgb = (static_cast(r) << 16 | static_cast(g) << 8 | static_cast(b)); - pointcloud->points[(size_t)depth_pixel_index].rgb = - *reinterpret_cast(&rgb); + pointcloud->points[(size_t)depth_pixel_index].rgb = *reinterpret_cast(&rgb); #else pointcloud->points[(size_t)depth_pixel_index].r = (uint8_t)96; pointcloud->points[(size_t)depth_pixel_index].g = (uint8_t)157; @@ -517,62 +473,32 @@ void vpRealSense2::getPointcloud( #if PCL_VERSION_COMPARE(<, 1, 1, 0) uint32_t rgb = 0; if (swap_rgb) { - rgb = (static_cast( - p_color_frame[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel]) | - static_cast( - p_color_frame[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 1]) - << 8 | - static_cast( - p_color_frame[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 2]) - << 16); + rgb = + (static_cast(p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]) | + static_cast(p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 | + static_cast(p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]) << 16); } else { - rgb = (static_cast( - p_color_frame[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel]) - << 16 | - static_cast( - p_color_frame[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 1]) - << 8 | - static_cast( - p_color_frame[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 2])); + rgb = (static_cast(p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]) << 16 | + static_cast(p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 | + static_cast(p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2])); } - pointcloud->points[(size_t)(i * depth_width + j)].rgb = - *reinterpret_cast(&rgb); + pointcloud->points[(size_t)(i * depth_width + j)].rgb = *reinterpret_cast(&rgb); #else if (swap_rgb) { pointcloud->points[(size_t)depth_pixel_index].b = - (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel]; + (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]; pointcloud->points[(size_t)depth_pixel_index].g = - (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 1]; + (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]; pointcloud->points[(size_t)depth_pixel_index].r = - (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 2]; + (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]; } else { pointcloud->points[(size_t)depth_pixel_index].r = - (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel]; + (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]; pointcloud->points[(size_t)depth_pixel_index].g = - (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 1]; + (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]; pointcloud->points[(size_t)depth_pixel_index].b = - (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 2]; + (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]; } #endif } @@ -587,9 +513,7 @@ void vpRealSense2::getPointcloud( has to be called after open(). \param from, to : streams for which the camera extrinsic parameters are returned. */ -vpHomogeneousMatrix -vpRealSense2::getTransformation(const rs2_stream &from, - const rs2_stream &to) const +vpHomogeneousMatrix vpRealSense2::getTransformation(const rs2_stream &from, const rs2_stream &to) const { auto from_stream = m_pipelineProfile.get_stream(from); auto to_stream = m_pipelineProfile.get_stream(to); @@ -624,14 +548,11 @@ void vpRealSense2::open(const rs2::config &cfg) } } - auto depth_stream = m_pipelineProfile.get_stream(RS2_STREAM_DEPTH) - .as(); + auto depth_stream = m_pipelineProfile.get_stream(RS2_STREAM_DEPTH).as(); m_depthIntrinsics = depth_stream.get_intrinsics(); auto color_stream = m_pipelineProfile.get_stream(RS2_STREAM_COLOR); - m_colorIntrinsics = m_pipelineProfile.get_stream(RS2_STREAM_COLOR) - .as() - .get_intrinsics(); + m_colorIntrinsics = m_pipelineProfile.get_stream(RS2_STREAM_COLOR).as().get_intrinsics(); m_depth2ColorExtrinsics = depth_stream.get_extrinsics_to(color_stream); } @@ -645,16 +566,13 @@ void print(const rs2_extrinsics &extrinsics, std::ostream &os) for (auto i = 0; i < 3; ++i) { for (auto j = 0; j < 3; ++j) { - ss << std::left << std::setw(15) << std::setprecision(5) - << extrinsics.rotation[j * 3 + i]; + ss << std::left << std::setw(15) << std::setprecision(5) << extrinsics.rotation[j * 3 + i]; } ss << std::endl; } ss << "\nTranslation Vector: "; - for (size_t i = 0; - i < sizeof(extrinsics.translation) / sizeof(extrinsics.translation[0]); - ++i) + for (size_t i = 0; i < sizeof(extrinsics.translation) / sizeof(extrinsics.translation[0]); ++i) ss << std::setprecision(15) << extrinsics.translation[i] << " "; os << ss.str() << "\n\n"; @@ -679,15 +597,13 @@ void print(const rs2_intrinsics &intrinsics, std::ostream &os) << "\t" << rs2_distortion_to_string(intrinsics.model) << "\n" << std::left << std::setw(14) << "Coeffs: "; - for (size_t i = 0; - i < sizeof(intrinsics.coeffs) / sizeof(intrinsics.coeffs[0]); ++i) + for (size_t i = 0; i < sizeof(intrinsics.coeffs) / sizeof(intrinsics.coeffs[0]); ++i) ss << "\t" << std::setprecision(15) << intrinsics.coeffs[i] << " "; os << ss.str() << "\n\n"; } -void safe_get_intrinsics(const rs2::video_stream_profile &profile, - rs2_intrinsics &intrinsics) +void safe_get_intrinsics(const rs2::video_stream_profile &profile, rs2_intrinsics &intrinsics) { try { intrinsics = profile.get_intrinsics(); @@ -697,9 +613,8 @@ void safe_get_intrinsics(const rs2::video_stream_profile &profile, bool operator==(const rs2_intrinsics &lhs, const rs2_intrinsics &rhs) { - return lhs.width == rhs.width && lhs.height == rhs.height && - lhs.ppx == rhs.ppx && lhs.ppy == rhs.ppy && lhs.fx == rhs.fx && - lhs.fy == rhs.fy && lhs.model == rhs.model && + return lhs.width == rhs.width && lhs.height == rhs.height && lhs.ppx == rhs.ppx && lhs.ppy == rhs.ppy && + lhs.fx == rhs.fx && lhs.fy == rhs.fy && lhs.model == rhs.model && !std::memcmp(lhs.coeffs, rhs.coeffs, sizeof(rhs.coeffs)); } @@ -707,10 +622,7 @@ std::string get_str_formats(const std::set &formats) { std::stringstream ss; for (auto format = formats.begin(); format != formats.end(); ++format) { - ss << *format - << ((format != formats.end()) && (next(format) == formats.end()) - ? "" - : "/"); + ss << *format << ((format != formats.end()) && (next(format) == formats.end()) ? "" : "/"); } return ss.str(); } @@ -724,9 +636,8 @@ struct stream_and_resolution { bool operator<(const stream_and_resolution &obj) const { - return ( - std::make_tuple(stream, stream_index, width, height) < - std::make_tuple(obj.stream, obj.stream_index, obj.width, obj.height)); + return (std::make_tuple(stream, stream_index, width, height) < + std::make_tuple(obj.stream, obj.stream_index, obj.width, obj.height)); } }; @@ -736,8 +647,7 @@ struct stream_and_index { bool operator<(const stream_and_index &obj) const { - return (std::make_tuple(stream, stream_index) < - std::make_tuple(obj.stream, obj.stream_index)); + return (std::make_tuple(stream, stream_index) < std::make_tuple(obj.stream, obj.stream_index)); } }; } // anonymous namespace @@ -764,13 +674,12 @@ int main() */ std::ostream &operator<<(std::ostream &os, const vpRealSense2 &rs) { - os << std::left << std::setw(30) << "Device Name" << std::setw(20) - << "Serial Number" << std::setw(20) << "Firmware Version" << std::endl; + os << std::left << std::setw(30) << "Device Name" << std::setw(20) << "Serial Number" << std::setw(20) + << "Firmware Version" << std::endl; rs2::device dev = rs.m_pipelineProfile.get_device(); - os << std::left << std::setw(30) << dev.get_info(RS2_CAMERA_INFO_NAME) - << std::setw(20) << dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER) - << std::setw(20) << dev.get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION) + os << std::left << std::setw(30) << dev.get_info(RS2_CAMERA_INFO_NAME) << std::setw(20) + << dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER) << std::setw(20) << dev.get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION) << std::endl; // Show which options are supported by this device @@ -778,28 +687,23 @@ std::ostream &operator<<(std::ostream &os, const vpRealSense2 &rs) for (auto j = 0; j < RS2_CAMERA_INFO_COUNT; ++j) { auto param = static_cast(j); if (dev.supports(param)) - os << " " << std::left << std::setw(30) - << rs2_camera_info_to_string(rs2_camera_info(param)) << ": \t" + os << " " << std::left << std::setw(30) << rs2_camera_info_to_string(rs2_camera_info(param)) << ": \t" << dev.get_info(param) << "\n"; } os << "\n"; for (auto &&sensor : dev.query_sensors()) { - os << "Options for " << sensor.get_info(RS2_CAMERA_INFO_NAME) - << std::endl; + os << "Options for " << sensor.get_info(RS2_CAMERA_INFO_NAME) << std::endl; - os << std::setw(55) << " Supported options:" << std::setw(10) << "min" - << std::setw(10) << " max" << std::setw(6) << " step" << std::setw(10) - << " default" << std::endl; + os << std::setw(55) << " Supported options:" << std::setw(10) << "min" << std::setw(10) << " max" << std::setw(6) + << " step" << std::setw(10) << " default" << std::endl; for (auto j = 0; j < RS2_OPTION_COUNT; ++j) { auto opt = static_cast(j); if (sensor.supports(opt)) { auto range = sensor.get_option_range(opt); - os << " " << std::left << std::setw(50) << opt << " : " - << std::setw(5) << range.min << "... " << std::setw(12) - << range.max << std::setw(6) << range.step << std::setw(10) - << range.def << "\n"; + os << " " << std::left << std::setw(50) << opt << " : " << std::setw(5) << range.min << "... " + << std::setw(12) << range.max << std::setw(6) << range.step << std::setw(10) << range.def << "\n"; } } @@ -807,22 +711,18 @@ std::ostream &operator<<(std::ostream &os, const vpRealSense2 &rs) } for (auto &&sensor : dev.query_sensors()) { - os << "Stream Profiles supported by " - << sensor.get_info(RS2_CAMERA_INFO_NAME) << "\n"; + os << "Stream Profiles supported by " << sensor.get_info(RS2_CAMERA_INFO_NAME) << "\n"; - os << std::setw(55) << " Supported modes:" << std::setw(10) << "stream" - << std::setw(10) << " resolution" << std::setw(6) << " fps" - << std::setw(10) << " format" + os << std::setw(55) << " Supported modes:" << std::setw(10) << "stream" << std::setw(10) << " resolution" + << std::setw(6) << " fps" << std::setw(10) << " format" << "\n"; // Show which streams are supported by this device for (auto &&profile : sensor.get_stream_profiles()) { if (auto video = profile.as()) { - os << " " << profile.stream_name() << "\t " << video.width() - << "x" << video.height() << "\t@ " << profile.fps() << "Hz\t" - << profile.format() << "\n"; + os << " " << profile.stream_name() << "\t " << video.width() << "x" << video.height() << "\t@ " + << profile.fps() << "Hz\t" << profile.format() << "\n"; } else { - os << " " << profile.stream_name() << "\t@ " << profile.fps() - << "Hz\t" << profile.format() << "\n"; + os << " " << profile.stream_name() << "\t@ " << profile.fps() << "Hz\t" << profile.format() << "\n"; } } @@ -830,34 +730,24 @@ std::ostream &operator<<(std::ostream &os, const vpRealSense2 &rs) } std::map streams; - std::map, rs2_intrinsics> > > - intrinsics_map; + std::map, rs2_intrinsics> > > intrinsics_map; for (auto &&sensor : dev.query_sensors()) { // Intrinsics for (auto &&profile : sensor.get_stream_profiles()) { if (auto video = profile.as()) { - if (streams.find(stream_and_index{profile.stream_type(), - profile.stream_index()}) == - streams.end()) { - streams[stream_and_index{profile.stream_type(), - profile.stream_index()}] = profile; + if (streams.find(stream_and_index{profile.stream_type(), profile.stream_index()}) == streams.end()) { + streams[stream_and_index{profile.stream_type(), profile.stream_index()}] = profile; } rs2_intrinsics intrinsics{}; - stream_and_resolution stream_res{ - profile.stream_type(), profile.stream_index(), video.width(), - video.height(), profile.stream_name()}; + stream_and_resolution stream_res{profile.stream_type(), profile.stream_index(), video.width(), video.height(), + profile.stream_name()}; safe_get_intrinsics(video, intrinsics); auto it = std::find_if( - (intrinsics_map[stream_res]).begin(), - (intrinsics_map[stream_res]).end(), - [&](const std::pair, rs2_intrinsics> &kvp) { - return intrinsics == kvp.second; - }); + (intrinsics_map[stream_res]).begin(), (intrinsics_map[stream_res]).end(), + [&](const std::pair, rs2_intrinsics> &kvp) { return intrinsics == kvp.second; }); if (it == (intrinsics_map[stream_res]).end()) { - (intrinsics_map[stream_res]) - .push_back({{profile.format()}, intrinsics}); + (intrinsics_map[stream_res]).push_back({{profile.format()}, intrinsics}); } else { it->first.insert(profile.format()); // If the intrinsics are equals, // add the profile format to @@ -872,9 +762,8 @@ std::ostream &operator<<(std::ostream &os, const vpRealSense2 &rs) auto stream_res = kvp.first; for (auto &intrinsics : kvp.second) { auto formats = get_str_formats(intrinsics.first); - os << "Intrinsic of \"" << stream_res.stream_name << "\"\t " - << stream_res.width << "x" << stream_res.height << "\t " << formats - << "\n"; + os << "Intrinsic of \"" << stream_res.stream_name << "\"\t " << stream_res.width << "x" << stream_res.height + << "\t " << formats << "\n"; if (intrinsics.second == rs2_intrinsics{}) { os << "Intrinsic NOT available!\n\n"; } else { diff --git a/modules/sensor/src/rgb-depth/realsense/vpRealSense_impl.h b/modules/sensor/src/rgb-depth/realsense/vpRealSense_impl.h index 45e6b60d35..8a4f6d283c 100644 --- a/modules/sensor/src/rgb-depth/realsense/vpRealSense_impl.h +++ b/modules/sensor/src/rgb-depth/realsense/vpRealSense_impl.h @@ -48,15 +48,12 @@ #endif template -void vp_rs_get_frame_data_impl( - const rs::device *m_device, - const std::map &m_intrinsics, - const rs::stream &stream, vpImage &data) +void vp_rs_get_frame_data_impl(const rs::device *m_device, const std::map &m_intrinsics, + const rs::stream &stream, vpImage &data) { // Retrieve data from stream if (m_device->is_stream_enabled(stream)) { - std::map::const_iterator it_intrinsics = - m_intrinsics.find(stream); + std::map::const_iterator it_intrinsics = m_intrinsics.find(stream); if (it_intrinsics == m_intrinsics.end()) { std::stringstream ss; ss << "Cannot find intrinsics for " << stream << " stream!"; @@ -67,34 +64,28 @@ void vp_rs_get_frame_data_impl( unsigned int height = (unsigned int)it_intrinsics->second.height; data.resize(height, width); - memcpy((unsigned char *)data.bitmap, - (unsigned char *)m_device->get_frame_data(stream), + memcpy((unsigned char *)data.bitmap, (unsigned char *)m_device->get_frame_data(stream), width * height * sizeof(Type)); } else { - throw vpException(vpException::fatalError, - "RealSense Camera - stream %d not enabled!", - (rs_stream)stream); + throw vpException(vpException::fatalError, "RealSense Camera - stream %d not enabled!", (rs_stream)stream); } } // Retrieve data from native stream -void vp_rs_get_native_frame_data_impl( - const rs::device *m_device, - const std::map &m_intrinsics, - const rs::stream &native_stream, unsigned char *const data, - const rs::stream &stream) +void vp_rs_get_native_frame_data_impl(const rs::device *m_device, + const std::map &m_intrinsics, + const rs::stream &native_stream, unsigned char *const data, + const rs::stream &stream) { if (m_device->is_stream_enabled(native_stream)) { - std::map::const_iterator it_intrinsics = - m_intrinsics.find(native_stream); + std::map::const_iterator it_intrinsics = m_intrinsics.find(native_stream); if (it_intrinsics == m_intrinsics.end()) { std::stringstream ss; ss << "Cannot find intrinsics for " << native_stream << " stream!"; throw vpException(vpException::fatalError, ss.str()); } - size_t size = - (size_t)(it_intrinsics->second.width * it_intrinsics->second.height); + size_t size = (size_t)(it_intrinsics->second.width * it_intrinsics->second.height); switch (m_device->get_stream_format(native_stream)) { // 8 bits @@ -107,52 +98,44 @@ void vp_rs_get_native_frame_data_impl( // Four 10-bit luminance values encoded into a 5-byte macropixel case rs::format::raw10: std::cout << "Stream: raw10 not tested!" << std::endl; - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), - (5 * (size / 4))); + memcpy(data, (unsigned char *)m_device->get_frame_data(stream), (5 * (size / 4))); break; // 16 bits case rs::format::raw16: std::cout << "Stream: raw16 not tested!" << std::endl; - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), - size * 2); + memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 2); break; case rs::format::disparity16: std::cout << "Stream: disparity16 not tested!" << std::endl; - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), - size * 2); + memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 2); break; case rs::format::y16: case rs::format::z16: - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), - size * 2); + memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 2); break; // 24 bits case rs::format::bgr8: case rs::format::rgb8: - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), - size * 3); + memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 3); break; // 32 bits case rs::format::yuyv: std::cout << "Stream: yuyv not tested!" << std::endl; - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), - size * 4); + memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 4); break; case rs::format::bgra8: case rs::format::rgba8: - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), - size * 4); + memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 4); break; // 96 bits case rs::format::xyz32f: std::cout << "Stream: xyz32f not tested!" << std::endl; - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), - size * 3 * 4); + memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 3 * 4); break; case rs::format::any: @@ -160,24 +143,18 @@ void vp_rs_get_native_frame_data_impl( break; } } else { - throw vpException(vpException::fatalError, - "RealSense Camera - stream %d not enabled!", - (rs_stream)native_stream); + throw vpException(vpException::fatalError, "RealSense Camera - stream %d not enabled!", (rs_stream)native_stream); } } // Retrieve color image -void vp_rs_get_color_impl( - const rs::device *m_device, - const std::map &m_intrinsics, - vpImage &color) +void vp_rs_get_color_impl(const rs::device *m_device, const std::map &m_intrinsics, + vpImage &color) { if (m_device->is_stream_enabled(rs::stream::color)) { - std::map::const_iterator it_intrinsics = - m_intrinsics.find(rs::stream::color); + std::map::const_iterator it_intrinsics = m_intrinsics.find(rs::stream::color); if (it_intrinsics == m_intrinsics.end()) { - throw vpException(vpException::fatalError, - "Cannot find intrinsics for color stream!"); + throw vpException(vpException::fatalError, "Cannot find intrinsics for color stream!"); } unsigned int width = (unsigned int)it_intrinsics->second.width; @@ -185,41 +162,30 @@ void vp_rs_get_color_impl( color.resize(height, width); if (m_device->get_stream_format(rs::stream::color) == rs::format::rgb8) { - vpImageConvert::RGBToRGBa( - (unsigned char *)m_device->get_frame_data(rs::stream::color), - (unsigned char *)color.bitmap, width, height); - } else if (m_device->get_stream_format(rs::stream::color) == - rs::format::rgba8) { - memcpy((unsigned char *)color.bitmap, - (unsigned char *)m_device->get_frame_data(rs::stream::color), + vpImageConvert::RGBToRGBa((unsigned char *)m_device->get_frame_data(rs::stream::color), + (unsigned char *)color.bitmap, width, height); + } else if (m_device->get_stream_format(rs::stream::color) == rs::format::rgba8) { + memcpy((unsigned char *)color.bitmap, (unsigned char *)m_device->get_frame_data(rs::stream::color), width * height * sizeof(vpRGBa)); - } else if (m_device->get_stream_format(rs::stream::color) == - rs::format::bgr8) { - vpImageConvert::BGRToRGBa( - (unsigned char *)m_device->get_frame_data(rs::stream::color), - (unsigned char *)color.bitmap, width, height); + } else if (m_device->get_stream_format(rs::stream::color) == rs::format::bgr8) { + vpImageConvert::BGRToRGBa((unsigned char *)m_device->get_frame_data(rs::stream::color), + (unsigned char *)color.bitmap, width, height); } else { - throw vpException(vpException::fatalError, - "RealSense Camera - color stream not supported!"); + throw vpException(vpException::fatalError, "RealSense Camera - color stream not supported!"); } } else { - throw vpException(vpException::fatalError, - "RealSense Camera - color stream not enabled!"); + throw vpException(vpException::fatalError, "RealSense Camera - color stream not enabled!"); } } // Retrieve grey image -void vp_rs_get_grey_impl( - const rs::device *m_device, - const std::map &m_intrinsics, - vpImage &grey) +void vp_rs_get_grey_impl(const rs::device *m_device, const std::map &m_intrinsics, + vpImage &grey) { if (m_device->is_stream_enabled(rs::stream::color)) { - std::map::const_iterator it_intrinsics = - m_intrinsics.find(rs::stream::color); + std::map::const_iterator it_intrinsics = m_intrinsics.find(rs::stream::color); if (it_intrinsics == m_intrinsics.end()) { - throw vpException(vpException::fatalError, - "Cannot find intrinsics for color stream!"); + throw vpException(vpException::fatalError, "Cannot find intrinsics for color stream!"); } unsigned int width = (unsigned int)it_intrinsics->second.width; @@ -227,43 +193,31 @@ void vp_rs_get_grey_impl( grey.resize(height, width); if (m_device->get_stream_format(rs::stream::color) == rs::format::rgb8) { - vpImageConvert::RGBToGrey( - (unsigned char *)m_device->get_frame_data(rs::stream::color), - (unsigned char *)grey.bitmap, width, height); - } else if (m_device->get_stream_format(rs::stream::color) == - rs::format::rgba8) { - vpImageConvert::RGBaToGrey( - (unsigned char *)m_device->get_frame_data(rs::stream::color), - (unsigned char *)grey.bitmap, width * height); - } else if (m_device->get_stream_format(rs::stream::color) == - rs::format::bgr8) { - vpImageConvert::BGRToGrey( - (unsigned char *)m_device->get_frame_data(rs::stream::color), - (unsigned char *)grey.bitmap, width, height); + vpImageConvert::RGBToGrey((unsigned char *)m_device->get_frame_data(rs::stream::color), + (unsigned char *)grey.bitmap, width, height); + } else if (m_device->get_stream_format(rs::stream::color) == rs::format::rgba8) { + vpImageConvert::RGBaToGrey((unsigned char *)m_device->get_frame_data(rs::stream::color), + (unsigned char *)grey.bitmap, width * height); + } else if (m_device->get_stream_format(rs::stream::color) == rs::format::bgr8) { + vpImageConvert::BGRToGrey((unsigned char *)m_device->get_frame_data(rs::stream::color), + (unsigned char *)grey.bitmap, width, height); } else { - throw vpException(vpException::fatalError, - "RealSense Camera - color stream not supported!"); + throw vpException(vpException::fatalError, "RealSense Camera - color stream not supported!"); } } else { - throw vpException(vpException::fatalError, - "RealSense Camera - color stream not enabled!"); + throw vpException(vpException::fatalError, "RealSense Camera - color stream not enabled!"); } } // Retrieve point cloud -void vp_rs_get_pointcloud_impl( - const rs::device *m_device, - const std::map &m_intrinsics, float max_Z, - std::vector &pointcloud, - const float invalidDepthValue = 0.0f, - const rs::stream &stream_depth = rs::stream::depth) +void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::map &m_intrinsics, + float max_Z, std::vector &pointcloud, const float invalidDepthValue = 0.0f, + const rs::stream &stream_depth = rs::stream::depth) { if (m_device->is_stream_enabled(rs::stream::depth)) { - std::map::const_iterator it_intrinsics = - m_intrinsics.find(stream_depth); + std::map::const_iterator it_intrinsics = m_intrinsics.find(stream_depth); if (it_intrinsics == m_intrinsics.end()) { - throw vpException(vpException::fatalError, - "Cannot find intrinsics for depth stream!"); + throw vpException(vpException::fatalError, "Cannot find intrinsics for depth stream!"); } const float depth_scale = m_device->get_depth_scale(); @@ -280,8 +234,7 @@ void vp_rs_get_pointcloud_impl( float scaled_depth = depth[i * width + j] * depth_scale; rs::float2 depth_pixel = {(float)j, (float)i}; - depth_point = - it_intrinsics->second.deproject(depth_pixel, scaled_depth); + depth_point = it_intrinsics->second.deproject(depth_pixel, scaled_depth); if (depth_point.z <= 0 || depth_point.z > max_Z) { depth_point.x = depth_point.y = depth_point.z = invalidDepthValue; @@ -301,19 +254,14 @@ void vp_rs_get_pointcloud_impl( #ifdef VISP_HAVE_PCL // Retrieve point cloud -void vp_rs_get_pointcloud_impl( - const rs::device *m_device, - const std::map &m_intrinsics, float max_Z, - pcl::PointCloud::Ptr &pointcloud, - const float invalidDepthValue = 0.0f, - const rs::stream &stream_depth = rs::stream::depth) +void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::map &m_intrinsics, + float max_Z, pcl::PointCloud::Ptr &pointcloud, + const float invalidDepthValue = 0.0f, const rs::stream &stream_depth = rs::stream::depth) { if (m_device->is_stream_enabled(rs::stream::depth)) { - std::map::const_iterator it_intrinsics = - m_intrinsics.find(stream_depth); + std::map::const_iterator it_intrinsics = m_intrinsics.find(stream_depth); if (it_intrinsics == m_intrinsics.end()) { - throw vpException(vpException::fatalError, - "Cannot find intrinsics for depth stream!"); + throw vpException(vpException::fatalError, "Cannot find intrinsics for depth stream!"); } int width = it_intrinsics->second.width; @@ -333,8 +281,7 @@ void vp_rs_get_pointcloud_impl( float scaled_depth = depth[i * width + j] * depth_scale; rs::float2 depth_pixel = {(float)j, (float)i}; - depth_point = - it_intrinsics->second.deproject(depth_pixel, scaled_depth); + depth_point = it_intrinsics->second.deproject(depth_pixel, scaled_depth); if (depth_point.z <= 0 || depth_point.z > max_Z) { depth_point.x = depth_point.y = depth_point.z = invalidDepthValue; @@ -350,37 +297,27 @@ void vp_rs_get_pointcloud_impl( } // Retrieve point cloud -void vp_rs_get_pointcloud_impl( - const rs::device *m_device, - const std::map &m_intrinsics, float max_Z, - pcl::PointCloud::Ptr &pointcloud, - const float invalidDepthValue = 0.0f, - const rs::stream &stream_color = rs::stream::color, - const rs::stream &stream_depth = rs::stream::depth) +void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::map &m_intrinsics, + float max_Z, pcl::PointCloud::Ptr &pointcloud, + const float invalidDepthValue = 0.0f, const rs::stream &stream_color = rs::stream::color, + const rs::stream &stream_depth = rs::stream::depth) { - if (m_device->is_stream_enabled(rs::stream::depth) && - m_device->is_stream_enabled(rs::stream::color)) { - std::map::const_iterator it_intrinsics_depth = - m_intrinsics.find(stream_depth); + if (m_device->is_stream_enabled(rs::stream::depth) && m_device->is_stream_enabled(rs::stream::color)) { + std::map::const_iterator it_intrinsics_depth = m_intrinsics.find(stream_depth); if (it_intrinsics_depth == m_intrinsics.end()) { - throw vpException(vpException::fatalError, - "Cannot find intrinsics for depth stream!"); + throw vpException(vpException::fatalError, "Cannot find intrinsics for depth stream!"); } - std::map::const_iterator it_intrinsics_color = - m_intrinsics.find(stream_color); + std::map::const_iterator it_intrinsics_color = m_intrinsics.find(stream_color); if (it_intrinsics_color == m_intrinsics.end()) { - throw vpException(vpException::fatalError, - "Cannot find intrinsics for color stream!"); + throw vpException(vpException::fatalError, "Cannot find intrinsics for color stream!"); } if (m_device->get_stream_format(rs::stream::color) != rs::format::rgb8 && m_device->get_stream_format(rs::stream::color) != rs::format::rgba8 && m_device->get_stream_format(rs::stream::color) != rs::format::bgr8 && m_device->get_stream_format(rs::stream::color) != rs::format::bgra8) { - throw vpException( - vpException::fatalError, - "Color stream type must be rgb8, rgba8, bgr8 or bgra8!"); + throw vpException(vpException::fatalError, "Color stream type must be rgb8, rgba8, bgr8 or bgra8!"); } int depth_width = it_intrinsics_depth->second.width; @@ -390,24 +327,19 @@ void vp_rs_get_pointcloud_impl( pointcloud->resize((size_t)(depth_width * depth_height)); const float depth_scale = m_device->get_depth_scale(); - rs::extrinsics depth_2_color_extrinsic = - m_device->get_extrinsics(stream_depth, stream_color); + rs::extrinsics depth_2_color_extrinsic = m_device->get_extrinsics(stream_depth, stream_color); - const uint16_t *depth = - (uint16_t *)m_device->get_frame_data(stream_depth); - const unsigned char *color = - (unsigned char *)m_device->get_frame_data(stream_color); + const uint16_t *depth = (uint16_t *)m_device->get_frame_data(stream_depth); + const unsigned char *color = (unsigned char *)m_device->get_frame_data(stream_color); int color_width = it_intrinsics_color->second.width; int color_height = it_intrinsics_color->second.height; - bool swap_rgb = - m_device->get_stream_format(rs::stream::color) == rs::format::bgr8 || - m_device->get_stream_format(rs::stream::color) == rs::format::bgra8; - unsigned int nb_color_pixel = - (m_device->get_stream_format(rs::stream::color) == rs::format::rgb8 || - m_device->get_stream_format(rs::stream::color) == rs::format::bgr8) - ? 3 - : 4; + bool swap_rgb = m_device->get_stream_format(rs::stream::color) == rs::format::bgr8 || + m_device->get_stream_format(rs::stream::color) == rs::format::bgra8; + unsigned int nb_color_pixel = (m_device->get_stream_format(rs::stream::color) == rs::format::rgb8 || + m_device->get_stream_format(rs::stream::color) == rs::format::bgr8) + ? 3 + : 4; int nb_threads = (int)std::thread::hardware_concurrency(); int step = depth_height / nb_threads; @@ -415,8 +347,7 @@ void vp_rs_get_pointcloud_impl( std::vector workers; for (int index = 0; index < nb_threads; index++) { int start_index = index * step; - int end_index = - index == nb_threads - 1 ? depth_height : (index + 1) * step; + int end_index = index == nb_threads - 1 ? depth_height : (index + 1) * step; workers.push_back(std::thread([&, start_index, end_index]() { for (int i = start_index; i < end_index; i++) { @@ -424,45 +355,33 @@ void vp_rs_get_pointcloud_impl( float scaled_depth = depth[i * depth_width + j] * depth_scale; rs::float2 depth_pixel = {(float)j, (float)i}; - rs::float3 depth_point = it_intrinsics_depth->second.deproject( - depth_pixel, scaled_depth); + rs::float3 depth_point = it_intrinsics_depth->second.deproject(depth_pixel, scaled_depth); if (depth_point.z <= 0 || depth_point.z > max_Z) { - depth_point.x = depth_point.y = depth_point.z = - invalidDepthValue; + depth_point.x = depth_point.y = depth_point.z = invalidDepthValue; } - pointcloud->points[(size_t)(i * depth_width + j)].x = - depth_point.x; - pointcloud->points[(size_t)(i * depth_width + j)].y = - depth_point.y; - pointcloud->points[(size_t)(i * depth_width + j)].z = - depth_point.z; - - rs::float3 color_point = - depth_2_color_extrinsic.transform(depth_point); - rs::float2 color_pixel = - it_intrinsics_color->second.project(color_point); - - if (color_pixel.y < 0 || color_pixel.y >= color_height || - color_pixel.x < 0 || color_pixel.x >= color_width) { + pointcloud->points[(size_t)(i * depth_width + j)].x = depth_point.x; + pointcloud->points[(size_t)(i * depth_width + j)].y = depth_point.y; + pointcloud->points[(size_t)(i * depth_width + j)].z = depth_point.z; + + rs::float3 color_point = depth_2_color_extrinsic.transform(depth_point); + rs::float2 color_pixel = it_intrinsics_color->second.project(color_point); + + if (color_pixel.y < 0 || color_pixel.y >= color_height || color_pixel.x < 0 || + color_pixel.x >= color_width) { // For out of bounds color data, default to a shade of blue in order to // visually distinguish holes. This color value is same as the librealsense // out of bounds color value. #if PCL_VERSION_COMPARE(<, 1, 1, 0) unsigned int r = 96, g = 157, b = 198; uint32_t rgb = - (static_cast(r) << 16 | - static_cast(g) << 8 | static_cast(b)); + (static_cast(r) << 16 | static_cast(g) << 8 | static_cast(b)); - pointcloud->points[(size_t)(i * depth_width + j)].rgb = - *reinterpret_cast(&rgb); + pointcloud->points[(size_t)(i * depth_width + j)].rgb = *reinterpret_cast(&rgb); #else - pointcloud->points[(size_t)(i * depth_width + j)].r = - (uint8_t)96; - pointcloud->points[(size_t)(i * depth_width + j)].g = - (uint8_t)157; - pointcloud->points[(size_t)(i * depth_width + j)].b = - (uint8_t)198; + pointcloud->points[(size_t)(i * depth_width + j)].r = (uint8_t)96; + pointcloud->points[(size_t)(i * depth_width + j)].g = (uint8_t)157; + pointcloud->points[(size_t)(i * depth_width + j)].b = (uint8_t)198; #endif } else { unsigned int i_ = (unsigned int)color_pixel.y; @@ -471,62 +390,31 @@ void vp_rs_get_pointcloud_impl( #if PCL_VERSION_COMPARE(<, 1, 1, 0) uint32_t rgb = 0; if (swap_rgb) { - rgb = (static_cast( - color[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel]) | - static_cast( - color[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 1]) - << 8 | - static_cast( - color[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 2]) - << 16); + rgb = (static_cast(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]) | + static_cast(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 | + static_cast(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]) << 16); } else { - rgb = (static_cast( - color[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel]) - << 16 | - static_cast( - color[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 1]) - << 8 | - static_cast( - color[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 2])); + rgb = (static_cast(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]) << 16 | + static_cast(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 | + static_cast(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2])); } - pointcloud->points[(size_t)(i * depth_width + j)].rgb = - *reinterpret_cast(&rgb); + pointcloud->points[(size_t)(i * depth_width + j)].rgb = *reinterpret_cast(&rgb); #else if (swap_rgb) { pointcloud->points[(size_t)(i * depth_width + j)].b = - (uint32_t)color[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel]; + (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]; pointcloud->points[(size_t)(i * depth_width + j)].g = - (uint32_t)color[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 1]; + (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]; pointcloud->points[(size_t)(i * depth_width + j)].r = - (uint32_t)color[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 2]; + (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]; } else { pointcloud->points[(size_t)(i * depth_width + j)].r = - (uint32_t)color[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel]; + (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]; pointcloud->points[(size_t)(i * depth_width + j)].g = - (uint32_t)color[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 1]; + (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]; pointcloud->points[(size_t)(i * depth_width + j)].b = - (uint32_t)color[(i_ * (unsigned int)color_width + j_) * - nb_color_pixel + - 2]; + (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]; } #endif } @@ -535,8 +423,7 @@ void vp_rs_get_pointcloud_impl( })); } - std::for_each(workers.begin(), workers.end(), - [](std::thread &t) { t.join(); }); + std::for_each(workers.begin(), workers.end(), [](std::thread &t) { t.join(); }); } else { pointcloud->clear(); } diff --git a/modules/sensor/test/force-torque/testComedi.cpp b/modules/sensor/test/force-torque/testComedi.cpp index 0537e80e99..2b81d40cea 100644 --- a/modules/sensor/test/force-torque/testComedi.cpp +++ b/modules/sensor/test/force-torque/testComedi.cpp @@ -59,14 +59,10 @@ int main() #ifdef VISP_HAVE_DISPLAY vpPlot scope(1, 700, 700, 100, 200, - std::string("ATI physical sensor data (") + - comedi.getPhyDataUnits() + std::string(")")); + std::string("ATI physical sensor data (") + comedi.getPhyDataUnits() + std::string(")")); scope.initGraph(0, comedi.getNChannel()); for (unsigned int i = 0; i < comedi.getNChannel(); i++) - scope.setLegend( - 0, i, - "G" + dynamic_cast((std::ostringstream() << i)) - .str()); + scope.setLegend(0, i, "G" + dynamic_cast((std::ostringstream() << i)).str()); #endif std::string file("recorded-physical-data-sync.txt"); @@ -74,12 +70,10 @@ int main() double start_time = vpTime::measureTimeMs(); #ifdef VISP_HAVE_DISPLAY - while ( - !vpDisplay::getClick(scope.I, false)) // Stop recording by a user click + while (!vpDisplay::getClick(scope.I, false)) // Stop recording by a user click #else std::cout << "Data recording during 20 seconds in progress..." << std::endl; - while (vpTime::measureTimeMs() - start_time < - 20000) // Stop recording after 20 seconds + while (vpTime::measureTimeMs() - start_time < 20000) // Stop recording after 20 seconds #endif { double loop_time = vpTime::measureTimeMs(); @@ -97,7 +91,6 @@ int main() f.close(); #else - std::cout << "You should install comedi to enable this test..." - << std::endl; + std::cout << "You should install comedi to enable this test..." << std::endl; #endif } diff --git a/modules/sensor/test/force-torque/testForceTorqueAti.cpp b/modules/sensor/test/force-torque/testForceTorqueAti.cpp index e139cbc6b6..5bae55e6b2 100644 --- a/modules/sensor/test/force-torque/testForceTorqueAti.cpp +++ b/modules/sensor/test/force-torque/testForceTorqueAti.cpp @@ -112,22 +112,16 @@ vpThread::Return scopeFunction(vpThread::Args args) scope.plot(0, shared_data.timestamp, force); scope.plot(1, shared_data.timestamp, torque); - vpDisplay::displayText(scope.I, 15, 500, "Left click to quit", - vpColor::red); - vpDisplay::displayText(scope.I, 30, 500, "Right click to bias/unbias", - vpColor::red); + vpDisplay::displayText(scope.I, 15, 500, "Left click to quit", vpColor::red); + vpDisplay::displayText(scope.I, 30, 500, "Right click to bias/unbias", vpColor::red); if (shared_data.bias_state == BIAS_DONE) - vpDisplay::displayText(scope.I, 45, 500, "Sensor is biased...", - vpColor::blue); + vpDisplay::displayText(scope.I, 45, 500, "Sensor is biased...", vpColor::blue); else if (shared_data.bias_state == UNBIAS_DONE) - vpDisplay::displayText(scope.I, 45, 500, "Sensor is unbiased...", - vpColor::blue); + vpDisplay::displayText(scope.I, 45, 500, "Sensor is unbiased...", vpColor::blue); else if (shared_data.bias_state == TO_BIAS) - vpDisplay::displayText(scope.I, 45, 500, "Sensor bias in progress...", - vpColor::blue); + vpDisplay::displayText(scope.I, 45, 500, "Sensor bias in progress...", vpColor::blue); else if (shared_data.bias_state == TO_UNBIAS) - vpDisplay::displayText(scope.I, 45, 500, "Sensor unbias in progress...", - vpColor::blue); + vpDisplay::displayText(scope.I, 45, 500, "Sensor unbias in progress...", vpColor::blue); vpDisplay::flush(scope.I); click = vpDisplay::getClick(scope.I, button, false); if (click && button == vpMouseButton::button3) { @@ -144,13 +138,9 @@ vpThread::Return scopeFunction(vpThread::Args args) #endif } #ifdef VISP_HAVE_DISPLAY - while ( - !(click && - button == - vpMouseButton::button1)); // Stop recording by a user left click + while (!(click && button == vpMouseButton::button1)); // Stop recording by a user left click #else - while (vpTime::measureTimeMs() - start_time < - 20000); // Stop recording after 20 seconds + while (vpTime::measureTimeMs() - start_time < 20000); // Stop recording after 20 seconds #endif { // Update state to stop @@ -169,16 +159,14 @@ int main(int argc, char **argv) #ifdef VISP_HAVE_VIPER850_DATA (void)argc; (void)argv; - std::string calibfile = - std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal"); + std::string calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal"); if (!vpIoTools::checkFilename(calibfile)) { std::cout << "ATI F/T calib file \"" << calibfile << "\" doesn't exist"; return 0; } #else if (argc != 2) { - std::cout << "Usage: " << argv[0] << " " - << std::endl; + std::cout << "Usage: " << argv[0] << " " << std::endl; return -1; } std::string calibfile(argv[1]); @@ -245,16 +233,10 @@ int main(int argc, char **argv) #else (void)argc; (void)argv; - std::cout - << "You should install comedi and build atidaq to enable this test..." - << std::endl; + std::cout << "You should install comedi and build atidaq to enable this test..." << std::endl; #endif } #else -int main() -{ - std::cout << "You should build this test with threading capabilities..." - << std::endl; -} +int main() { std::cout << "You should build this test with threading capabilities..." << std::endl; } #endif diff --git a/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp b/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp index 72daffcc48..e4a881452e 100644 --- a/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp +++ b/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp @@ -84,8 +84,7 @@ int main() 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; + 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) { diff --git a/modules/sensor/test/framegrabber/testPylonGrabber.cpp b/modules/sensor/test/framegrabber/testPylonGrabber.cpp index 53245174c3..bbf515577e 100644 --- a/modules/sensor/test/framegrabber/testPylonGrabber.cpp +++ b/modules/sensor/test/framegrabber/testPylonGrabber.cpp @@ -72,8 +72,7 @@ int main() vpPylonFactory &factory = vpPylonFactory::instance(); // Creation of a framegrabber - vpPylonGrabber *g = - factory.createPylonGrabber(vpPylonFactory::BASLER_GIGE); + vpPylonGrabber *g = factory.createPylonGrabber(vpPylonFactory::BASLER_GIGE); std::string guid; // Get the number of cameras connected on the bus @@ -106,8 +105,7 @@ int main() for (int i = 0; i < 10; i++) g->acquire(I); g->close(); - std::cout << "Current image size: " << g->getWidth() << "x" - << g->getHeight() << std::endl; + std::cout << "Current image size: " << g->getWidth() << "x" << g->getHeight() << std::endl; std::string filename = outputpath + "/imagetest1.pgm"; std::cout << "Write image: " << filename << std::endl; diff --git a/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp b/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp index f1b0621ea4..037a2bd8f5 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp @@ -42,8 +42,7 @@ #include -#if defined(VISP_HAVE_REALSENSE2) && \ - defined(VISP_HAVE_CPP11_COMPATIBILITY) && \ +#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && \ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) #include @@ -67,31 +66,22 @@ namespace { #ifdef VISP_HAVE_PCL // Global variables -pcl::PointCloud::Ptr - pointcloud(new pcl::PointCloud()); -pcl::PointCloud::Ptr - pointcloud_color(new pcl::PointCloud()); +pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud()); +pcl::PointCloud::Ptr pointcloud_color(new pcl::PointCloud()); bool cancelled = false, update_pointcloud = false; class ViewerWorker { public: - explicit ViewerWorker(const bool color_mode, std::mutex &mutex) - : m_colorMode(color_mode), m_mutex(mutex) - { - } + explicit ViewerWorker(const bool color_mode, std::mutex &mutex) : m_colorMode(color_mode), m_mutex(mutex) {} void run() { std::string date = vpTime::getDateTime(); - pcl::visualization::PCLVisualizer::Ptr viewer( - new pcl::visualization::PCLVisualizer("3D Viewer " + date)); - pcl::visualization::PointCloudColorHandlerRGBField rgb( - pointcloud_color); - pcl::PointCloud::Ptr local_pointcloud( - new pcl::PointCloud()); - pcl::PointCloud::Ptr local_pointcloud_color( - new pcl::PointCloud()); + pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer " + date)); + pcl::visualization::PointCloudColorHandlerRGBField rgb(pointcloud_color); + pcl::PointCloud::Ptr local_pointcloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr local_pointcloud_color(new pcl::PointCloud()); viewer->setBackgroundColor(0, 0, 0); viewer->initCameraParameters(); @@ -125,26 +115,19 @@ class ViewerWorker if (init) { if (m_colorMode) { - viewer->addPointCloud(local_pointcloud_color, - rgb, "RGB sample cloud"); - viewer->setPointCloudRenderingProperties( - pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, - "RGB sample cloud"); + viewer->addPointCloud(local_pointcloud_color, rgb, "RGB sample cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, + "RGB sample cloud"); } else { - viewer->addPointCloud(local_pointcloud, - "sample cloud"); - viewer->setPointCloudRenderingProperties( - pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, - "sample cloud"); + viewer->addPointCloud(local_pointcloud, "sample cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); } init = false; } else { if (m_colorMode) { - viewer->updatePointCloud( - local_pointcloud_color, rgb, "RGB sample cloud"); + viewer->updatePointCloud(local_pointcloud_color, rgb, "RGB sample cloud"); } else { - viewer->updatePointCloud(local_pointcloud, - "sample cloud"); + viewer->updatePointCloud(local_pointcloud, "sample cloud"); } } } @@ -160,8 +143,7 @@ class ViewerWorker std::mutex &m_mutex; }; -void getPointcloud(const rs2::depth_frame &depth_frame, - std::vector &pointcloud) +void getPointcloud(const rs2::depth_frame &depth_frame, std::vector &pointcloud) { auto vf = depth_frame.as(); const int width = vf.get_width(); @@ -268,24 +250,16 @@ int main(int argc, char *argv[]) rs.open(config); rs2::pipeline_profile &profile = rs.getPipelineProfile(); - auto color_profile = - profile.get_stream(RS2_STREAM_COLOR).as(); - vpImage color((unsigned int)color_profile.height(), - (unsigned int)color_profile.width()); - - auto depth_profile = - profile.get_stream(RS2_STREAM_DEPTH).as(); - vpImage depth_color((unsigned int)depth_profile.height(), - (unsigned int)depth_profile.width()); - vpImage depth_raw((unsigned int)depth_profile.height(), - (unsigned int)depth_profile.width()); - - auto infrared_profile = - profile.get_stream(RS2_STREAM_INFRARED).as(); - vpImage infrared((unsigned int)infrared_profile.height(), - (unsigned int)infrared_profile.width()); - vpImage infrared_raw((unsigned int)infrared_profile.height(), - (unsigned int)infrared_profile.width()); + auto color_profile = profile.get_stream(RS2_STREAM_COLOR).as(); + vpImage color((unsigned int)color_profile.height(), (unsigned int)color_profile.width()); + + auto depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as(); + vpImage depth_color((unsigned int)depth_profile.height(), (unsigned int)depth_profile.width()); + vpImage depth_raw((unsigned int)depth_profile.height(), (unsigned int)depth_profile.width()); + + auto infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as(); + vpImage infrared((unsigned int)infrared_profile.height(), (unsigned int)infrared_profile.width()); + vpImage infrared_raw((unsigned int)infrared_profile.height(), (unsigned int)infrared_profile.width()); #ifdef VISP_HAVE_X11 vpDisplayX d1, d2, d3; @@ -306,44 +280,26 @@ int main(int argc, char *argv[]) rs2::pipeline &pipe = rs.getPipeline(); std::cout << "Color intrinsics:\n" - << rs.getCameraParameters( - RS2_STREAM_COLOR, - vpCameraParameters::perspectiveProjWithoutDistortion) + << rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithoutDistortion) << std::endl; std::cout << "Color intrinsics:\n" - << rs.getCameraParameters( - RS2_STREAM_COLOR, - vpCameraParameters::perspectiveProjWithDistortion) - << std::endl; + << rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithDistortion) << std::endl; std::cout << "Depth intrinsics:\n" - << rs.getCameraParameters( - RS2_STREAM_DEPTH, - vpCameraParameters::perspectiveProjWithoutDistortion) + << rs.getCameraParameters(RS2_STREAM_DEPTH, vpCameraParameters::perspectiveProjWithoutDistortion) << std::endl; std::cout << "Depth intrinsics:\n" - << rs.getCameraParameters( - RS2_STREAM_DEPTH, - vpCameraParameters::perspectiveProjWithDistortion) - << std::endl; + << rs.getCameraParameters(RS2_STREAM_DEPTH, vpCameraParameters::perspectiveProjWithDistortion) << std::endl; std::cout << "Infrared intrinsics:\n" - << rs.getCameraParameters( - RS2_STREAM_INFRARED, - vpCameraParameters::perspectiveProjWithoutDistortion) + << rs.getCameraParameters(RS2_STREAM_INFRARED, vpCameraParameters::perspectiveProjWithoutDistortion) << std::endl; std::cout << "Infrared intrinsics:\n" - << rs.getCameraParameters( - RS2_STREAM_INFRARED, - vpCameraParameters::perspectiveProjWithDistortion) + << rs.getCameraParameters(RS2_STREAM_INFRARED, vpCameraParameters::perspectiveProjWithDistortion) << std::endl; - std::cout << "depth_M_color:\n" - << rs.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) - << std::endl; - std::cout << "color_M_infrared:\n" - << rs.getTransformation(RS2_STREAM_INFRARED, RS2_STREAM_COLOR) - << std::endl; + std::cout << "depth_M_color:\n" << rs.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) << std::endl; + std::cout << "color_M_infrared:\n" << rs.getTransformation(RS2_STREAM_INFRARED, RS2_STREAM_COLOR) << std::endl; std::vector time_vector; double t_begin = vpTime::measureTimeMs(); @@ -411,8 +367,7 @@ int main(int argc, char *argv[]) viewer_colvector_thread.join(); #endif std::cout << "Acquisition1 - Mean time: " << vpMath::getMean(time_vector) - << " ms ; Median time: " << vpMath::getMedian(time_vector) - << " ms" << std::endl; + << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl; config.disable_all_streams(); config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 60); @@ -420,22 +375,15 @@ int main(int argc, char *argv[]) config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 60); rs.open(config); - color_profile = - profile.get_stream(RS2_STREAM_COLOR).as(); - color.init((unsigned int)color_profile.height(), - (unsigned int)color_profile.width()); + color_profile = profile.get_stream(RS2_STREAM_COLOR).as(); + color.init((unsigned int)color_profile.height(), (unsigned int)color_profile.width()); - depth_profile = - profile.get_stream(RS2_STREAM_DEPTH).as(); - depth_color.init((unsigned int)depth_profile.height(), - (unsigned int)depth_profile.width()); - depth_raw.init((unsigned int)depth_profile.height(), - (unsigned int)depth_profile.width()); + depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as(); + depth_color.init((unsigned int)depth_profile.height(), (unsigned int)depth_profile.width()); + depth_raw.init((unsigned int)depth_profile.height(), (unsigned int)depth_profile.width()); - infrared_profile = - profile.get_stream(RS2_STREAM_INFRARED).as(); - infrared.init((unsigned int)infrared_profile.height(), - (unsigned int)infrared_profile.width()); + infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as(); + infrared.init((unsigned int)infrared_profile.height(), (unsigned int)infrared_profile.width()); d1.init(color, 0, 0, "Color"); d2.init(depth_color, color.getWidth(), 0, "Depth"); @@ -449,44 +397,26 @@ int main(int argc, char *argv[]) std::cout << "\n" << std::endl; std::cout << "Color intrinsics:\n" - << rs.getCameraParameters( - RS2_STREAM_COLOR, - vpCameraParameters::perspectiveProjWithoutDistortion) + << rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithoutDistortion) << std::endl; std::cout << "Color intrinsics:\n" - << rs.getCameraParameters( - RS2_STREAM_COLOR, - vpCameraParameters::perspectiveProjWithDistortion) - << std::endl; + << rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithDistortion) << std::endl; std::cout << "Depth intrinsics:\n" - << rs.getCameraParameters( - RS2_STREAM_DEPTH, - vpCameraParameters::perspectiveProjWithoutDistortion) + << rs.getCameraParameters(RS2_STREAM_DEPTH, vpCameraParameters::perspectiveProjWithoutDistortion) << std::endl; std::cout << "Depth intrinsics:\n" - << rs.getCameraParameters( - RS2_STREAM_DEPTH, - vpCameraParameters::perspectiveProjWithDistortion) - << std::endl; + << rs.getCameraParameters(RS2_STREAM_DEPTH, vpCameraParameters::perspectiveProjWithDistortion) << std::endl; std::cout << "Infrared intrinsics:\n" - << rs.getCameraParameters( - RS2_STREAM_INFRARED, - vpCameraParameters::perspectiveProjWithoutDistortion) + << rs.getCameraParameters(RS2_STREAM_INFRARED, vpCameraParameters::perspectiveProjWithoutDistortion) << std::endl; std::cout << "Infrared intrinsics:\n" - << rs.getCameraParameters( - RS2_STREAM_INFRARED, - vpCameraParameters::perspectiveProjWithDistortion) + << rs.getCameraParameters(RS2_STREAM_INFRARED, vpCameraParameters::perspectiveProjWithDistortion) << std::endl; - std::cout << "depth_M_color:\n" - << rs.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) - << std::endl; - std::cout << "color_M_infrared:\n" - << rs.getTransformation(RS2_STREAM_INFRARED, RS2_STREAM_COLOR) - << std::endl; + std::cout << "depth_M_color:\n" << rs.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) << std::endl; + std::cout << "color_M_infrared:\n" << rs.getTransformation(RS2_STREAM_INFRARED, RS2_STREAM_COLOR) << std::endl; time_vector.clear(); t_begin = vpTime::measureTimeMs(); @@ -498,20 +428,17 @@ int main(int argc, char *argv[]) std::lock_guard lock(mutex); if (pcl_color) { - rs.acquire((unsigned char *)color.bitmap, - (unsigned char *)depth_raw.bitmap, NULL, pointcloud_color, + rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth_raw.bitmap, NULL, pointcloud_color, (unsigned char *)infrared.bitmap); } else { - rs.acquire((unsigned char *)color.bitmap, - (unsigned char *)depth_raw.bitmap, NULL, pointcloud, + rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth_raw.bitmap, NULL, pointcloud, (unsigned char *)infrared.bitmap); } update_pointcloud = true; } #else - rs.acquire((unsigned char *)color.bitmap, - (unsigned char *)depth_raw.bitmap, NULL, + rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth_raw.bitmap, NULL, (unsigned char *)infrared.bitmap); #endif @@ -545,8 +472,7 @@ int main(int argc, char *argv[]) d2.close(depth_color); d3.close(infrared); std::cout << "Acquisition2 - Mean time: " << vpMath::getMean(time_vector) - << " ms ; Median time: " << vpMath::getMedian(time_vector) - << " ms" << std::endl; + << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl; #if VISP_HAVE_OPENCV_VERSION >= 0x030000 rs.close(); @@ -556,19 +482,15 @@ int main(int argc, char *argv[]) config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 60); rs.open(config); - color_profile = - profile.get_stream(RS2_STREAM_COLOR).as(); + color_profile = profile.get_stream(RS2_STREAM_COLOR).as(); cv::Mat mat_color(color_profile.height(), color_profile.width(), CV_8UC3); - depth_profile = - profile.get_stream(RS2_STREAM_DEPTH).as(); + depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as(); cv::Mat mat_depth(depth_profile.height(), depth_profile.width(), CV_8UC3); rs2::colorizer color_map; - infrared_profile = - profile.get_stream(RS2_STREAM_INFRARED).as(); - cv::Mat mat_infrared(infrared_profile.height(), infrared_profile.width(), - CV_8U); + infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as(); + cv::Mat mat_infrared(infrared_profile.height(), infrared_profile.width(), CV_8U); time_vector.clear(); t_begin = vpTime::measureTimeMs(); @@ -590,8 +512,7 @@ int main(int argc, char *argv[]) } std::cout << "Acquisition3 - Mean time: " << vpMath::getMean(time_vector) - << " ms ; Median time: " << vpMath::getMedian(time_vector) - << " ms" << std::endl; + << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl; #endif return EXIT_SUCCESS; diff --git a/modules/sensor/test/rgb-depth/testRealSense_R200.cpp b/modules/sensor/test/rgb-depth/testRealSense_R200.cpp index 501099c4b9..193cd3d632 100644 --- a/modules/sensor/test/rgb-depth/testRealSense_R200.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense_R200.cpp @@ -46,8 +46,7 @@ #include #include -#if defined(VISP_HAVE_REALSENSE) && \ - defined(VISP_HAVE_CPP11_COMPATIBILITY) && \ +#if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && \ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) #include #include @@ -62,31 +61,22 @@ namespace #ifdef VISP_HAVE_PCL // Global variables -pcl::PointCloud::Ptr - pointcloud(new pcl::PointCloud()); -pcl::PointCloud::Ptr - pointcloud_color(new pcl::PointCloud()); +pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud()); +pcl::PointCloud::Ptr pointcloud_color(new pcl::PointCloud()); bool cancelled = false, update_pointcloud = false; class ViewerWorker { public: - explicit ViewerWorker(const bool color_mode, std::mutex &mutex) - : m_colorMode(color_mode), m_mutex(mutex) - { - } + explicit ViewerWorker(const bool color_mode, std::mutex &mutex) : m_colorMode(color_mode), m_mutex(mutex) {} void run() { std::string date = vpTime::getDateTime(); - pcl::visualization::PCLVisualizer::Ptr viewer( - new pcl::visualization::PCLVisualizer("3D Viewer " + date)); - pcl::visualization::PointCloudColorHandlerRGBField rgb( - pointcloud_color); - pcl::PointCloud::Ptr local_pointcloud( - new pcl::PointCloud()); - pcl::PointCloud::Ptr local_pointcloud_color( - new pcl::PointCloud()); + pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer " + date)); + pcl::visualization::PointCloudColorHandlerRGBField rgb(pointcloud_color); + pcl::PointCloud::Ptr local_pointcloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr local_pointcloud_color(new pcl::PointCloud()); viewer->setBackgroundColor(0, 0, 0); viewer->initCameraParameters(); @@ -120,26 +110,19 @@ class ViewerWorker if (init) { if (m_colorMode) { - viewer->addPointCloud(local_pointcloud_color, - rgb, "RGB sample cloud"); - viewer->setPointCloudRenderingProperties( - pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, - "RGB sample cloud"); + viewer->addPointCloud(local_pointcloud_color, rgb, "RGB sample cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, + "RGB sample cloud"); } else { - viewer->addPointCloud(local_pointcloud, - "sample cloud"); - viewer->setPointCloudRenderingProperties( - pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, - "sample cloud"); + viewer->addPointCloud(local_pointcloud, "sample cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); } init = false; } else { if (m_colorMode) { - viewer->updatePointCloud( - local_pointcloud_color, rgb, "RGB sample cloud"); + viewer->updatePointCloud(local_pointcloud_color, rgb, "RGB sample cloud"); } else { - viewer->updatePointCloud(local_pointcloud, - "sample cloud"); + viewer->updatePointCloud(local_pointcloud, "sample cloud"); } } } @@ -156,24 +139,20 @@ class ViewerWorker }; #endif //#ifdef VISP_HAVE_PCL -void test_R200( - vpRealSense &rs, const std::map &enables, - const std::map ¶ms, - const std::map &options, const std::string &title, - const bool depth_color_visualization = false, - const rs::stream &color_stream = rs::stream::color, - const rs::stream &depth_stream = rs::stream::depth, - const rs::stream &infrared2_stream = rs::stream::infrared2, - bool display_pcl = false, bool pcl_color = false) +void test_R200(vpRealSense &rs, const std::map &enables, + const std::map ¶ms, + const std::map &options, const std::string &title, + const bool depth_color_visualization = false, const rs::stream &color_stream = rs::stream::color, + const rs::stream &depth_stream = rs::stream::depth, + const rs::stream &infrared2_stream = rs::stream::infrared2, bool display_pcl = false, + bool pcl_color = false) { std::cout << std::endl; std::map::const_iterator it_enable; - std::map::const_iterator - it_param; + std::map::const_iterator it_param; - for (it_enable = enables.begin(), it_param = params.begin(); - it_enable != enables.end(); ++it_enable) { + for (it_enable = enables.begin(), it_param = params.begin(); it_enable != enables.end(); ++it_enable) { rs.setEnableStream(it_enable->first, it_enable->second); if (it_enable->second) { @@ -218,21 +197,18 @@ void test_R200( it_param = params.find(it_enable->first); if (it_param != params.end()) { - direct_infrared_conversion = - it_param->second.m_streamFormat == rs::format::y8; + direct_infrared_conversion = it_param->second.m_streamFormat == rs::format::y8; } break; case rs::stream::infrared2: - infrared2.init( - (unsigned int)rs.getIntrinsics(it_enable->first).height, - (unsigned int)rs.getIntrinsics(it_enable->first).width); + infrared2.init((unsigned int)rs.getIntrinsics(it_enable->first).height, + (unsigned int)rs.getIntrinsics(it_enable->first).width); I_infrared2.init(infrared.getHeight(), infrared.getWidth()); it_param = params.find(it_enable->first); if (it_param != params.end()) { - direct_infrared_conversion = - it_param->second.m_streamFormat == rs::format::y8; + direct_infrared_conversion = it_param->second.m_streamFormat == rs::format::y8; } break; @@ -257,25 +233,19 @@ void test_R200( case rs::stream::depth: if (depth_color_visualization) { - dd.init(I_depth_color, (int)I_color.getWidth() + 80, 0, - "Depth frame"); + dd.init(I_depth_color, (int)I_color.getWidth() + 80, 0, "Depth frame"); } else { dd.init(I_depth, (int)I_color.getWidth() + 80, 0, "Depth frame"); } break; case rs::stream::infrared: - di.init(I_infrared, 0, - (int)(std::max)(I_color.getHeight(), I_depth.getHeight()) + - 30, - "Infrared frame"); + di.init(I_infrared, 0, (int)(std::max)(I_color.getHeight(), I_depth.getHeight()) + 30, "Infrared frame"); break; case rs::stream::infrared2: di2.init(I_infrared2, (int)I_infrared.getWidth(), - (int)(std::max)(I_color.getHeight(), I_depth.getHeight()) + - 30, - "Infrared2 frame"); + (int)(std::max)(I_color.getHeight(), I_depth.getHeight()) + 30, "Infrared2 frame"); break; default: @@ -284,8 +254,7 @@ void test_R200( } } - std::cout << "direct_infrared_conversion=" << direct_infrared_conversion - << std::endl; + std::cout << "direct_infrared_conversion=" << direct_infrared_conversion << std::endl; #ifdef VISP_HAVE_PCL std::mutex mutex; @@ -307,8 +276,7 @@ void test_R200( while (true) { double t = vpTime::measureTimeMs(); - for (std::map::const_iterator it = options.begin(); - it != options.end(); ++it) { + for (std::map::const_iterator it = options.begin(); it != options.end(); ++it) { dev->set_option(it->first, it->second); } @@ -318,31 +286,23 @@ void test_R200( if (direct_infrared_conversion) { if (pcl_color) { - rs.acquire((unsigned char *)I_color.bitmap, - (unsigned char *)depth.bitmap, NULL, pointcloud_color, - (unsigned char *)I_infrared.bitmap, - (unsigned char *)I_infrared2.bitmap, color_stream, + rs.acquire((unsigned char *)I_color.bitmap, (unsigned char *)depth.bitmap, NULL, pointcloud_color, + (unsigned char *)I_infrared.bitmap, (unsigned char *)I_infrared2.bitmap, color_stream, depth_stream, rs::stream::infrared, infrared2_stream); } else { - rs.acquire((unsigned char *)I_color.bitmap, - (unsigned char *)depth.bitmap, NULL, pointcloud, - (unsigned char *)I_infrared.bitmap, - (unsigned char *)I_infrared2.bitmap, color_stream, + rs.acquire((unsigned char *)I_color.bitmap, (unsigned char *)depth.bitmap, NULL, pointcloud, + (unsigned char *)I_infrared.bitmap, (unsigned char *)I_infrared2.bitmap, color_stream, depth_stream, rs::stream::infrared, infrared2_stream); } } else { if (pcl_color) { - rs.acquire((unsigned char *)I_color.bitmap, - (unsigned char *)depth.bitmap, NULL, pointcloud_color, - (unsigned char *)infrared.bitmap, - (unsigned char *)infrared2.bitmap, color_stream, - depth_stream, rs::stream::infrared, infrared2_stream); + rs.acquire((unsigned char *)I_color.bitmap, (unsigned char *)depth.bitmap, NULL, pointcloud_color, + (unsigned char *)infrared.bitmap, (unsigned char *)infrared2.bitmap, color_stream, depth_stream, + rs::stream::infrared, infrared2_stream); } else { - rs.acquire((unsigned char *)I_color.bitmap, - (unsigned char *)depth.bitmap, NULL, pointcloud, - (unsigned char *)infrared.bitmap, - (unsigned char *)infrared2.bitmap, color_stream, - depth_stream, rs::stream::infrared, infrared2_stream); + rs.acquire((unsigned char *)I_color.bitmap, (unsigned char *)depth.bitmap, NULL, pointcloud, + (unsigned char *)infrared.bitmap, (unsigned char *)infrared2.bitmap, color_stream, depth_stream, + rs::stream::infrared, infrared2_stream); } vpImageConvert::convert(infrared, I_infrared); @@ -353,17 +313,13 @@ void test_R200( #endif } else { if (direct_infrared_conversion) { - rs.acquire((unsigned char *)I_color.bitmap, - (unsigned char *)depth.bitmap, NULL, - (unsigned char *)I_infrared.bitmap, - (unsigned char *)I_infrared2.bitmap, color_stream, - depth_stream, rs::stream::infrared, infrared2_stream); + rs.acquire((unsigned char *)I_color.bitmap, (unsigned char *)depth.bitmap, NULL, + (unsigned char *)I_infrared.bitmap, (unsigned char *)I_infrared2.bitmap, color_stream, depth_stream, + rs::stream::infrared, infrared2_stream); } else { - rs.acquire((unsigned char *)I_color.bitmap, - (unsigned char *)depth.bitmap, NULL, - (unsigned char *)infrared.bitmap, - (unsigned char *)infrared2.bitmap, color_stream, - depth_stream, rs::stream::infrared, infrared2_stream); + rs.acquire((unsigned char *)I_color.bitmap, (unsigned char *)depth.bitmap, NULL, + (unsigned char *)infrared.bitmap, (unsigned char *)infrared2.bitmap, color_stream, depth_stream, + rs::stream::infrared, infrared2_stream); vpImageConvert::convert(infrared, I_infrared); vpImageConvert::convert(infrared2, I_infrared2); } @@ -394,10 +350,8 @@ void test_R200( vpDisplay::flush(I_infrared2); if (vpDisplay::getClick(I_color, false) || - (depth_color_visualization ? vpDisplay::getClick(I_depth_color, false) - : vpDisplay::getClick(I_depth, false)) || - vpDisplay::getClick(I_infrared, false) || - vpDisplay::getClick(I_infrared2, false)) { + (depth_color_visualization ? vpDisplay::getClick(I_depth_color, false) : vpDisplay::getClick(I_depth, false)) || + vpDisplay::getClick(I_infrared, false) || vpDisplay::getClick(I_infrared2, false)) { break; } @@ -419,8 +373,7 @@ void test_R200( } std::cout << title << " - Mean time: " << vpMath::getMean(time_vector) - << " ms ; Median time: " << vpMath::getMedian(time_vector) - << " ms" << std::endl; + << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl; rs.close(); } @@ -434,18 +387,13 @@ int main(int argc, char *argv[]) rs.setEnableStream(rs::stream::color, false); rs.open(); - if (rs_get_device_name((const rs_device *)rs.getHandler(), nullptr) != - std::string("Intel RealSense R200")) { - std::cout - << "This test file is used to test the Intel RealSense R200 only." - << std::endl; + if (rs_get_device_name((const rs_device *)rs.getHandler(), nullptr) != std::string("Intel RealSense R200")) { + std::cout << "This test file is used to test the Intel RealSense R200 only." << std::endl; return EXIT_SUCCESS; } std::cout << "API version: " << rs_get_api_version(nullptr) << std::endl; - std::cout << "Firmware: " - << rs_get_device_firmware_version( - (const rs_device *)rs.getHandler(), nullptr) + std::cout << "Firmware: " << rs_get_device_firmware_version((const rs_device *)rs.getHandler(), nullptr) << std::endl; std::cout << "RealSense sensor characteristics: \n" << rs << std::endl; @@ -460,26 +408,20 @@ int main(int argc, char *argv[]) enables[rs::stream::infrared] = false; enables[rs::stream::infrared2] = false; - params[rs::stream::depth] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 90); + params[rs::stream::depth] = vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 90); options[rs::option::r200_lr_auto_exposure_enabled] = 1; - test_R200(rs, enables, params, options, - "R200_DEPTH_Z16_640x480_90FPS + r200_lr_auto_exposure_enabled", - true); + test_R200(rs, enables, params, options, "R200_DEPTH_Z16_640x480_90FPS + r200_lr_auto_exposure_enabled", true); enables[rs::stream::color] = false; enables[rs::stream::depth] = true; enables[rs::stream::infrared] = true; enables[rs::stream::infrared2] = true; - params[rs::stream::depth] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 90); - params[rs::stream::infrared] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 90); - params[rs::stream::infrared2] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 90); + params[rs::stream::depth] = vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 90); + params[rs::stream::infrared] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 90); + params[rs::stream::infrared2] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 90); options[rs::option::r200_lr_auto_exposure_enabled] = 0; options[rs::option::r200_emitter_enabled] = 0; @@ -495,12 +437,9 @@ int main(int argc, char *argv[]) enables[rs::stream::infrared] = true; enables[rs::stream::infrared2] = true; - params[rs::stream::depth] = - vpRealSense::vpRsStreamParams(628, 468, rs::format::z16, 90); - params[rs::stream::infrared] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y16, 90); - params[rs::stream::infrared2] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y16, 90); + params[rs::stream::depth] = vpRealSense::vpRsStreamParams(628, 468, rs::format::z16, 90); + params[rs::stream::infrared] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y16, 90); + params[rs::stream::infrared2] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y16, 90); options[rs::option::r200_lr_auto_exposure_enabled] = 1; options[rs::option::r200_emitter_enabled] = 1; @@ -515,12 +454,9 @@ int main(int argc, char *argv[]) enables[rs::stream::infrared] = true; enables[rs::stream::infrared2] = true; - params[rs::stream::depth] = - vpRealSense::vpRsStreamParams(628, 468, rs::format::z16, 90); - params[rs::stream::infrared] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 90); - params[rs::stream::infrared2] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 90); + params[rs::stream::depth] = vpRealSense::vpRsStreamParams(628, 468, rs::format::z16, 90); + params[rs::stream::infrared] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 90); + params[rs::stream::infrared2] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 90); options.clear(); @@ -533,14 +469,10 @@ int main(int argc, char *argv[]) enables[rs::stream::infrared] = true; enables[rs::stream::infrared2] = true; - params[rs::stream::color] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30); - params[rs::stream::depth] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 90); - params[rs::stream::infrared] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 90); - params[rs::stream::infrared2] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 90); + params[rs::stream::color] = vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30); + params[rs::stream::depth] = vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 90); + params[rs::stream::infrared] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 90); + params[rs::stream::infrared2] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 90); test_R200(rs, enables, params, options, "R200_COLOR_RGBA8_640x480_30FPS + R200_DEPTH_Z16_628x468_90FPS " @@ -553,19 +485,16 @@ int main(int argc, char *argv[]) enables[rs::stream::infrared] = false; enables[rs::stream::infrared2] = false; - params[rs::stream::color] = - vpRealSense::vpRsStreamParams(1920, 1080, rs::format::rgba8, 30); + params[rs::stream::color] = vpRealSense::vpRsStreamParams(1920, 1080, rs::format::rgba8, 30); - test_R200(rs, enables, params, options, - "R200_COLOR_RGBA8_1920x1080_30FPS"); + test_R200(rs, enables, params, options, "R200_COLOR_RGBA8_1920x1080_30FPS"); enables[rs::stream::color] = true; enables[rs::stream::depth] = false; enables[rs::stream::infrared] = false; enables[rs::stream::infrared2] = false; - params[rs::stream::color] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 60); + params[rs::stream::color] = vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 60); test_R200(rs, enables, params, options, "R200_COLOR_RGBA8_640x480_60FPS"); enables[rs::stream::color] = true; @@ -573,31 +502,22 @@ int main(int argc, char *argv[]) enables[rs::stream::infrared] = true; enables[rs::stream::infrared2] = true; - params[rs::stream::color] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 60); - params[rs::stream::depth] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60); - params[rs::stream::infrared] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60); - params[rs::stream::infrared2] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60); + params[rs::stream::color] = vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 60); + params[rs::stream::depth] = vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60); + params[rs::stream::infrared] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60); + params[rs::stream::infrared2] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60); // depth == 0 ; color == 1 ; infrared2 == 3 ; rectified_color == 6 ; // color_aligned_to_depth == 7 ; infrared2_aligned_to_depth == 8 ; // depth_aligned_to_color == 9 ; depth_aligned_to_rectified_color == 10 ; // depth_aligned_to_infrared2 == 11 argv[2] <==> color stream - rs::stream color_stream = argc > 2 ? (rs::stream)atoi(argv[2]) - : rs::stream::color_aligned_to_depth; + rs::stream color_stream = argc > 2 ? (rs::stream)atoi(argv[2]) : rs::stream::color_aligned_to_depth; std::cout << "\ncolor_stream: " << color_stream << std::endl; // argv[3] <==> depth stream - rs::stream depth_stream = - argc > 3 ? (rs::stream)atoi(argv[3]) - : rs::stream::depth_aligned_to_rectified_color; + rs::stream depth_stream = argc > 3 ? (rs::stream)atoi(argv[3]) : rs::stream::depth_aligned_to_rectified_color; std::cout << "depth_stream: " << depth_stream << std::endl; // argv[4] <==> depth stream - rs::stream infrared2_stream = - argc > 4 ? (rs::stream)atoi(argv[4]) - : rs::stream::infrared2_aligned_to_depth; + rs::stream infrared2_stream = argc > 4 ? (rs::stream)atoi(argv[4]) : rs::stream::infrared2_aligned_to_depth; std::cout << "infrared2_stream: " << infrared2_stream << std::endl; test_R200(rs, enables, params, options, @@ -607,39 +527,33 @@ int main(int argc, char *argv[]) "R200_INFRARED2_ALIGNED_TO_DEPTH_Y8_640x480_60FPS", true, color_stream, depth_stream, infrared2_stream); -#if (!defined(__APPLE__) && \ - !defined(__MACH__)) // Not OSX, since viewer->spinOnce (10); produces a - // segfault on OSX +#if (!defined(__APPLE__) && !defined(__MACH__)) // Not OSX, since viewer->spinOnce (10); produces a + // segfault on OSX enables[rs::stream::color] = true; enables[rs::stream::depth] = true; enables[rs::stream::infrared] = true; enables[rs::stream::infrared2] = true; - params[rs::stream::color] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 60); - params[rs::stream::depth] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60); - params[rs::stream::infrared] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60); - params[rs::stream::infrared2] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60); + params[rs::stream::color] = vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 60); + params[rs::stream::depth] = vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60); + params[rs::stream::infrared] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60); + params[rs::stream::infrared2] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60); // Cannot render two pcl::visualization::PCLVisualizer so use an arg // option to switch between B&W and color point cloud rendering until a // solution is found - test_R200( - rs, enables, params, options, - "R200_COLOR_RGBA8_640x480_60FPS + R200_DEPTH_Z16_640x480_60FPS + " - "R200_INFRARED_Y8_640x480_60FPS + R200_INFRARED2_Y8_640x480_60FPS", - false, rs::stream::color, rs::stream::depth, rs::stream::infrared2, - true, (argc > 1 ? (bool)(atoi(argv[1]) > 0) : false)); + test_R200(rs, enables, params, options, + "R200_COLOR_RGBA8_640x480_60FPS + R200_DEPTH_Z16_640x480_60FPS + " + "R200_INFRARED_Y8_640x480_60FPS + R200_INFRARED2_Y8_640x480_60FPS", + false, rs::stream::color, rs::stream::depth, rs::stream::infrared2, true, + (argc > 1 ? (bool)(atoi(argv[1]) > 0) : false)); #endif } catch (const vpException &e) { std::cerr << "RealSense error " << e.what() << std::endl; } catch (const rs::error &e) { - std::cerr << "RealSense error calling " << e.get_failed_function() << "(" - << e.get_failed_args() << "): " << e.what() << std::endl; + std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() + << "): " << e.what() << std::endl; } catch (const std::exception &e) { std::cerr << e.what() << std::endl; } diff --git a/modules/sensor/test/rgb-depth/testRealSense_SR300.cpp b/modules/sensor/test/rgb-depth/testRealSense_SR300.cpp index 323f04e15d..d9aead1929 100644 --- a/modules/sensor/test/rgb-depth/testRealSense_SR300.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense_SR300.cpp @@ -46,8 +46,7 @@ #include #include -#if defined(VISP_HAVE_REALSENSE) && \ - defined(VISP_HAVE_CPP11_COMPATIBILITY) && \ +#if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && \ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) #include #include @@ -62,31 +61,22 @@ namespace #ifdef VISP_HAVE_PCL // Global variables -pcl::PointCloud::Ptr - pointcloud(new pcl::PointCloud()); -pcl::PointCloud::Ptr - pointcloud_color(new pcl::PointCloud()); +pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud()); +pcl::PointCloud::Ptr pointcloud_color(new pcl::PointCloud()); bool cancelled = false, update_pointcloud = false; class ViewerWorker { public: - explicit ViewerWorker(const bool color_mode, std::mutex &mutex) - : m_colorMode(color_mode), m_mutex(mutex) - { - } + explicit ViewerWorker(const bool color_mode, std::mutex &mutex) : m_colorMode(color_mode), m_mutex(mutex) {} void run() { std::string date = vpTime::getDateTime(); - pcl::visualization::PCLVisualizer::Ptr viewer( - new pcl::visualization::PCLVisualizer("3D Viewer " + date)); - pcl::visualization::PointCloudColorHandlerRGBField rgb( - pointcloud_color); - pcl::PointCloud::Ptr local_pointcloud( - new pcl::PointCloud()); - pcl::PointCloud::Ptr local_pointcloud_color( - new pcl::PointCloud()); + pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer " + date)); + pcl::visualization::PointCloudColorHandlerRGBField rgb(pointcloud_color); + pcl::PointCloud::Ptr local_pointcloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr local_pointcloud_color(new pcl::PointCloud()); viewer->setBackgroundColor(0, 0, 0); viewer->initCameraParameters(); @@ -120,26 +110,19 @@ class ViewerWorker if (init) { if (m_colorMode) { - viewer->addPointCloud(local_pointcloud_color, - rgb, "RGB sample cloud"); - viewer->setPointCloudRenderingProperties( - pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, - "RGB sample cloud"); + viewer->addPointCloud(local_pointcloud_color, rgb, "RGB sample cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, + "RGB sample cloud"); } else { - viewer->addPointCloud(local_pointcloud, - "sample cloud"); - viewer->setPointCloudRenderingProperties( - pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, - "sample cloud"); + viewer->addPointCloud(local_pointcloud, "sample cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); } init = false; } else { if (m_colorMode) { - viewer->updatePointCloud( - local_pointcloud_color, rgb, "RGB sample cloud"); + viewer->updatePointCloud(local_pointcloud_color, rgb, "RGB sample cloud"); } else { - viewer->updatePointCloud(local_pointcloud, - "sample cloud"); + viewer->updatePointCloud(local_pointcloud, "sample cloud"); } } } @@ -156,22 +139,17 @@ class ViewerWorker }; #endif //#ifdef VISP_HAVE_PCL -void test_SR300( - vpRealSense &rs, const std::map &enables, - const std::map ¶ms, - const std::string &title, const bool depth_color_visualization = false, - const rs::stream &color_stream = rs::stream::color, - const rs::stream &depth_stream = rs::stream::depth, - bool display_pcl = false, bool pcl_color = false) +void test_SR300(vpRealSense &rs, const std::map &enables, + const std::map ¶ms, const std::string &title, + const bool depth_color_visualization = false, const rs::stream &color_stream = rs::stream::color, + const rs::stream &depth_stream = rs::stream::depth, bool display_pcl = false, bool pcl_color = false) { std::cout << std::endl; std::map::const_iterator it_enable; - std::map::const_iterator - it_param; + std::map::const_iterator it_param; - for (it_enable = enables.begin(), it_param = params.begin(); - it_enable != enables.end(); ++it_enable) { + for (it_enable = enables.begin(), it_param = params.begin(); it_enable != enables.end(); ++it_enable) { rs.setEnableStream(it_enable->first, it_enable->second); if (it_enable->second) { @@ -216,8 +194,7 @@ void test_SR300( it_param = params.find(it_enable->first); if (it_param != params.end()) { - direct_infrared_conversion = - it_param->second.m_streamFormat == rs::format::y8; + direct_infrared_conversion = it_param->second.m_streamFormat == rs::format::y8; } break; @@ -242,18 +219,14 @@ void test_SR300( case rs::stream::depth: if (depth_color_visualization) { - dd.init(I_depth_color, (int)I_color.getWidth() + 80, 0, - "Depth frame"); + dd.init(I_depth_color, (int)I_color.getWidth() + 80, 0, "Depth frame"); } else { dd.init(I_depth, (int)I_color.getWidth() + 80, 0, "Depth frame"); } break; case rs::stream::infrared: - di.init(I_infrared, 0, - (int)(std::max)(I_color.getHeight(), I_depth.getHeight()) + - 30, - "Infrared frame"); + di.init(I_infrared, 0, (int)(std::max)(I_color.getHeight(), I_depth.getHeight()) + 30, "Infrared frame"); break; default: @@ -263,8 +236,7 @@ void test_SR300( } if (rs.getHandler()->is_stream_enabled(rs::stream::infrared)) { - std::cout << "direct_infrared_conversion=" << direct_infrared_conversion - << std::endl; + std::cout << "direct_infrared_conversion=" << direct_infrared_conversion << std::endl; } #ifdef VISP_HAVE_PCL @@ -291,27 +263,19 @@ void test_SR300( if (direct_infrared_conversion) { if (pcl_color) { - rs.acquire((unsigned char *)I_color.bitmap, - (unsigned char *)depth.bitmap, NULL, pointcloud_color, - (unsigned char *)I_infrared.bitmap, NULL, color_stream, - depth_stream); + rs.acquire((unsigned char *)I_color.bitmap, (unsigned char *)depth.bitmap, NULL, pointcloud_color, + (unsigned char *)I_infrared.bitmap, NULL, color_stream, depth_stream); } else { - rs.acquire((unsigned char *)I_color.bitmap, - (unsigned char *)depth.bitmap, NULL, pointcloud, - (unsigned char *)I_infrared.bitmap, NULL, color_stream, - depth_stream); + rs.acquire((unsigned char *)I_color.bitmap, (unsigned char *)depth.bitmap, NULL, pointcloud, + (unsigned char *)I_infrared.bitmap, NULL, color_stream, depth_stream); } } else { if (pcl_color) { - rs.acquire((unsigned char *)I_color.bitmap, - (unsigned char *)depth.bitmap, NULL, pointcloud_color, - (unsigned char *)infrared.bitmap, NULL, color_stream, - depth_stream); + rs.acquire((unsigned char *)I_color.bitmap, (unsigned char *)depth.bitmap, NULL, pointcloud_color, + (unsigned char *)infrared.bitmap, NULL, color_stream, depth_stream); } else { - rs.acquire((unsigned char *)I_color.bitmap, - (unsigned char *)depth.bitmap, NULL, pointcloud, - (unsigned char *)infrared.bitmap, NULL, color_stream, - depth_stream); + rs.acquire((unsigned char *)I_color.bitmap, (unsigned char *)depth.bitmap, NULL, pointcloud, + (unsigned char *)infrared.bitmap, NULL, color_stream, depth_stream); } vpImageConvert::convert(infrared, I_infrared); @@ -321,15 +285,11 @@ void test_SR300( #endif } else { if (direct_infrared_conversion) { - rs.acquire((unsigned char *)I_color.bitmap, - (unsigned char *)depth.bitmap, NULL, - (unsigned char *)I_infrared.bitmap, NULL, color_stream, - depth_stream); + rs.acquire((unsigned char *)I_color.bitmap, (unsigned char *)depth.bitmap, NULL, + (unsigned char *)I_infrared.bitmap, NULL, color_stream, depth_stream); } else { - rs.acquire((unsigned char *)I_color.bitmap, - (unsigned char *)depth.bitmap, NULL, - (unsigned char *)infrared.bitmap, NULL, color_stream, - depth_stream); + rs.acquire((unsigned char *)I_color.bitmap, (unsigned char *)depth.bitmap, NULL, + (unsigned char *)infrared.bitmap, NULL, color_stream, depth_stream); vpImageConvert::convert(infrared, I_infrared); } } @@ -357,8 +317,7 @@ void test_SR300( vpDisplay::flush(I_infrared); if (vpDisplay::getClick(I_color, false) || - (depth_color_visualization ? vpDisplay::getClick(I_depth_color, false) - : vpDisplay::getClick(I_depth, false)) || + (depth_color_visualization ? vpDisplay::getClick(I_depth_color, false) : vpDisplay::getClick(I_depth, false)) || vpDisplay::getClick(I_infrared, false)) { break; } @@ -381,8 +340,7 @@ void test_SR300( } std::cout << title << " - Mean time: " << vpMath::getMean(time_vector) - << " ms ; Median time: " << vpMath::getMedian(time_vector) - << " ms" << std::endl; + << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl; rs.close(); } @@ -396,18 +354,13 @@ int main(int argc, char *argv[]) rs.setEnableStream(rs::stream::color, false); rs.open(); - if (rs_get_device_name((const rs_device *)rs.getHandler(), nullptr) != - std::string("Intel RealSense SR300")) { - std::cout - << "This test file is used to test the Intel RealSense SR300 only." - << std::endl; + if (rs_get_device_name((const rs_device *)rs.getHandler(), nullptr) != std::string("Intel RealSense SR300")) { + std::cout << "This test file is used to test the Intel RealSense SR300 only." << std::endl; return EXIT_SUCCESS; } std::cout << "API version: " << rs_get_api_version(nullptr) << std::endl; - std::cout << "Firmware: " - << rs_get_device_firmware_version( - (const rs_device *)rs.getHandler(), nullptr) + std::cout << "Firmware: " << rs_get_device_firmware_version((const rs_device *)rs.getHandler(), nullptr) << std::endl; std::cout << "RealSense sensor characteristics: \n" << rs << std::endl; @@ -420,8 +373,7 @@ int main(int argc, char *argv[]) enables[rs::stream::depth] = true; enables[rs::stream::infrared] = false; - params[rs::stream::depth] = - vpRealSense::vpRsStreamParams(640, 240, rs::format::z16, 110); + params[rs::stream::depth] = vpRealSense::vpRsStreamParams(640, 240, rs::format::z16, 110); test_SR300(rs, enables, params, "SR300_DEPTH_Z16_640x240_110FPS"); @@ -429,8 +381,7 @@ int main(int argc, char *argv[]) enables[rs::stream::depth] = false; enables[rs::stream::infrared] = false; - params[rs::stream::color] = - vpRealSense::vpRsStreamParams(1920, 1080, rs::format::rgba8, 30); + params[rs::stream::color] = vpRealSense::vpRsStreamParams(1920, 1080, rs::format::rgba8, 30); test_SR300(rs, enables, params, "SR300_COLOR_RGBA8_1920x1080_30FPS"); @@ -438,8 +389,7 @@ int main(int argc, char *argv[]) enables[rs::stream::depth] = false; enables[rs::stream::infrared] = true; - params[rs::stream::infrared] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y16, 200); + params[rs::stream::infrared] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y16, 200); test_SR300(rs, enables, params, "SR300_INFRARED_Y16_640x480_200FPS"); @@ -447,8 +397,7 @@ int main(int argc, char *argv[]) enables[rs::stream::depth] = true; enables[rs::stream::infrared] = false; - params[rs::stream::depth] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60); + params[rs::stream::depth] = vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60); test_SR300(rs, enables, params, "SR300_DEPTH_Z16_640x480_60FPS", true); @@ -456,40 +405,29 @@ int main(int argc, char *argv[]) enables[rs::stream::depth] = true; enables[rs::stream::infrared] = true; - params[rs::stream::depth] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60); - params[rs::stream::infrared] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60); + params[rs::stream::depth] = vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60); + params[rs::stream::infrared] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60); - test_SR300( - rs, enables, params, - "SR300_DEPTH_Z16_640x480_60FPS + SR300_INFRARED_Y8_640x480_60FPS", - true); + test_SR300(rs, enables, params, "SR300_DEPTH_Z16_640x480_60FPS + SR300_INFRARED_Y8_640x480_60FPS", true); -#if (!defined(__APPLE__) && \ - !defined(__MACH__)) // Not OSX, since viewer->spinOnce (10); produces a - // segfault on OSX +#if (!defined(__APPLE__) && !defined(__MACH__)) // Not OSX, since viewer->spinOnce (10); produces a + // segfault on OSX enables[rs::stream::color] = true; enables[rs::stream::depth] = true; enables[rs::stream::infrared] = true; - params[rs::stream::color] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 60); - params[rs::stream::depth] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60); - params[rs::stream::infrared] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60); + params[rs::stream::color] = vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 60); + params[rs::stream::depth] = vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60); + params[rs::stream::infrared] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60); // depth == 0 ; color == 1 ; rectified_color == 6 ; color_aligned_to_depth - // == 7 ; depth_aligned_to_color == 9 ; depth_aligned_to_rectified_color == - // 10 argv[1] <==> color stream - rs::stream color_stream = - argc > 1 ? (rs::stream)atoi(argv[1]) : rs::stream::color; + // == 7 ; depth_aligned_to_color == 9 ; depth_aligned_to_rectified_color + // == 10 argv[1] <==> color stream + rs::stream color_stream = argc > 1 ? (rs::stream)atoi(argv[1]) : rs::stream::color; std::cout << "\ncolor_stream: " << color_stream << std::endl; // argv[2] <==> depth stream - rs::stream depth_stream = - argc > 2 ? (rs::stream)atoi(argv[2]) : rs::stream::depth; + rs::stream depth_stream = argc > 2 ? (rs::stream)atoi(argv[2]) : rs::stream::depth; std::cout << "depth_stream: " << depth_stream << std::endl; test_SR300(rs, enables, params, @@ -504,10 +442,8 @@ int main(int argc, char *argv[]) enables[rs::stream::depth] = true; enables[rs::stream::infrared] = false; - params[rs::stream::color] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30); - params[rs::stream::depth] = - vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 30); + params[rs::stream::color] = vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30); + params[rs::stream::depth] = vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 30); test_SR300(rs, enables, params, "SR300_COLOR_ALIGNED_TO_DEPTH_RGBA8_640x480_30FPS + " @@ -524,12 +460,8 @@ int main(int argc, char *argv[]) rs.setEnableStream(rs::stream::color, true); rs.setEnableStream(rs::stream::depth, false); rs.setEnableStream(rs::stream::infrared, true); - rs.setStreamSettings( - rs::stream::color, - vpRealSense::vpRsStreamParams(640, 480, rs::format::bgr8, 60)); - rs.setStreamSettings( - rs::stream::infrared, - vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 200)); + rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, 480, rs::format::bgr8, 60)); + rs.setStreamSettings(rs::stream::infrared, vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 200)); rs.open(); cv::Mat color_mat(480, 640, CV_8UC3); @@ -555,8 +487,8 @@ int main(int argc, char *argv[]) } catch (const vpException &e) { std::cerr << "RealSense error " << e.what() << std::endl; } catch (const rs::error &e) { - std::cerr << "RealSense error calling " << e.get_failed_function() << "(" - << e.get_failed_args() << "): " << e.what() << std::endl; + std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() + << "): " << e.what() << std::endl; } catch (const std::exception &e) { std::cerr << e.what() << std::endl; } diff --git a/modules/tracker/blob/include/visp3/blob/vpDot.h b/modules/tracker/blob/include/visp3/blob/vpDot.h index 6570ca0426..ff3780669e 100644 --- a/modules/tracker/blob/include/visp3/blob/vpDot.h +++ b/modules/tracker/blob/include/visp3/blob/vpDot.h @@ -88,8 +88,8 @@ int main() { #if defined(VISP_HAVE_DC1394) vpImage 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 + 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"); @@ -129,77 +129,76 @@ class VISP_EXPORT vpDot : public vpTracker 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() - */ + 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(); explicit vpDot(const vpImagePoint &ip); vpDot(const vpDot &d); virtual ~vpDot(); - void display(const vpImage &I, vpColor color = vpColor::red, - unsigned int thickness = 1) const; + void display(const vpImage &I, vpColor color = vpColor::red, unsigned int thickness = 1) const; /*! @@ -212,8 +211,7 @@ class VISP_EXPORT vpDot : public vpTracker { vpRect bbox; - bbox.setRect(this->u_min, this->v_min, this->u_max - this->u_min + 1, - this->v_max - this->v_min + 1); + bbox.setRect(this->u_min, this->v_min, this->u_max - this->u_min + 1, this->v_max - this->v_min + 1); return (bbox); }; @@ -230,10 +228,7 @@ class VISP_EXPORT vpDot : public vpTracker \warning Doesn't return the image points inside the dot anymore. To get those points see getConnexities(). */ - inline std::list getEdges() const - { - return this->ip_edges_list; - }; + inline std::list getEdges() const { return this->ip_edges_list; }; /*! @@ -243,10 +238,7 @@ class VISP_EXPORT vpDot : public vpTracker This list is updated after a call to track(). */ - inline std::list getConnexities() const - { - return this->ip_connexities_list; - }; + inline std::list getConnexities() const { return this->ip_connexities_list; }; inline double getGamma() const { return this->gamma; }; /*! @@ -275,10 +267,7 @@ class VISP_EXPORT vpDot : public vpTracker \sa getHeight() */ - inline unsigned int getWidth() const - { - return (this->u_max - this->u_min + 1); - }; + inline unsigned int getWidth() const { return (this->u_max - this->u_min + 1); }; /*! @@ -287,15 +276,12 @@ class VISP_EXPORT vpDot : public vpTracker \sa getHeight() */ - inline unsigned int getHeight() const - { - return (this->v_max - this->v_min + 1); - }; + inline unsigned int getHeight() const { return (this->v_max - this->v_min + 1); }; void initTracking(const vpImage &I); void initTracking(const vpImage &I, const vpImagePoint &ip); - void initTracking(const vpImage &I, const vpImagePoint &ip, - unsigned int gray_level_min, unsigned int gray_level_max); + void initTracking(const vpImage &I, const vpImagePoint &ip, unsigned int gray_level_min, + unsigned int gray_level_max); vpDot &operator=(const vpDot &d); bool operator==(const vpDot &d) const; @@ -330,14 +316,8 @@ class VISP_EXPORT vpDot : public vpTracker */ 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 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); /*! @@ -406,25 +386,19 @@ class VISP_EXPORT vpDot : public vpTracker void init(); void setGrayLevelOut(); - bool connexe(const vpImage &I, unsigned int u, - unsigned int v, double &mean_value, double &u_cog, + bool connexe(const vpImage &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog, double &v_cog, double &n); - bool connexe(const vpImage &I, unsigned int u, - unsigned int v, double &mean_value, double &u_cog, + bool connexe(const vpImage &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog, double &v_cog, double &n, std::vector &checkTab); void COG(const vpImage &I, double &u, double &v); // Static Functions public: - static void display(const vpImage &I, - const vpImagePoint &cog, - const std::list &edges_list, - vpColor color = vpColor::red, - unsigned int thickness = 1); - static void display(const vpImage &I, const vpImagePoint &cog, - const std::list &edges_list, - vpColor color = vpColor::red, + static void display(const vpImage &I, const vpImagePoint &cog, + const std::list &edges_list, vpColor color = vpColor::red, unsigned int thickness = 1); + static void display(const vpImage &I, const vpImagePoint &cog, const std::list &edges_list, + vpColor color = vpColor::red, unsigned int thickness = 1); }; #endif diff --git a/modules/tracker/blob/include/visp3/blob/vpDot2.h b/modules/tracker/blob/include/visp3/blob/vpDot2.h index f8bcf034f5..24b46db2ab 100644 --- a/modules/tracker/blob/include/visp3/blob/vpDot2.h +++ b/modules/tracker/blob/include/visp3/blob/vpDot2.h @@ -131,14 +131,10 @@ class VISP_EXPORT vpDot2 : public vpTracker vpDot2(const vpDot2 &twinDot); virtual ~vpDot2(); - static vpMatrix defineDots(vpDot2 dot[], const unsigned int &n, - const std::string &dotFile, - vpImage &I, - vpColor col = vpColor::blue, - bool trackDot = true); + static vpMatrix defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage &I, + vpColor col = vpColor::blue, bool trackDot = true); - void display(const vpImage &I, vpColor color = vpColor::red, - unsigned int thickness = 1) const; + void display(const vpImage &I, vpColor color = vpColor::red, unsigned int thickness = 1) const; double getArea() const; /*! @@ -152,8 +148,7 @@ class VISP_EXPORT vpDot2 : public vpTracker { vpRect bbox; - bbox.setRect(this->bbox_u_min, this->bbox_v_min, - this->bbox_u_max - this->bbox_u_min + 1, + 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); @@ -175,10 +170,7 @@ class VISP_EXPORT vpDot2 : public vpTracker border. This list is update after a call to track(). */ - void getEdges(std::list &edges_list) const - { - edges_list = this->ip_edges_list; - }; + void getEdges(std::list &edges_list) const { edges_list = this->ip_edges_list; }; /*! Return the list of all the image points on the dot @@ -195,10 +187,7 @@ class VISP_EXPORT vpDot2 : public vpTracker \sa setEllipsoidBadPointsPercentage() */ - double getEllipsoidBadPointsPercentage() const - { - return allowedBadPointsPercentage_; - } + double getEllipsoidBadPointsPercentage() const { return allowedBadPointsPercentage_; } double getEllipsoidShapePrecision() const; void getFreemanChain(std::list &freeman_chain) const; @@ -232,22 +221,18 @@ class VISP_EXPORT vpDot2 : public vpTracker double getWidth() const; void initTracking(const vpImage &I, unsigned int size = 0); - void initTracking(const vpImage &I, const vpImagePoint &ip, - unsigned int size = 0); - void initTracking(const vpImage &I, const vpImagePoint &ip, - unsigned int gray_lvl_min, unsigned int gray_lvl_max, - unsigned int size = 0); + void initTracking(const vpImage &I, const vpImagePoint &ip, unsigned int size = 0); + void initTracking(const vpImage &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 &I, int area_u, - int area_v, unsigned int area_w, unsigned int area_h, - std::list &niceDots); + void searchDotsInArea(const vpImage &I, int area_u, int area_v, unsigned int area_w, + unsigned int area_h, std::list &niceDots); - void searchDotsInArea(const vpImage &I, - std::list &niceDots); + void searchDotsInArea(const vpImage &I, std::list &niceDots); void setArea(const double &area); /*! @@ -352,18 +337,15 @@ class VISP_EXPORT vpDot2 : public vpTracker }; void setGrayLevelPrecision(const double &grayLevelPrecision); void setHeight(const double &height); - void setMaxSizeSearchDistancePrecision( - const double &maxSizeSearchDistancePrecision); + void setMaxSizeSearchDistancePrecision(const double &maxSizeSearchDistancePrecision); void setSizePrecision(const double &sizePrecision); void setWidth(const double &width); void track(const vpImage &I); void track(const vpImage &I, vpImagePoint &cog); - static void trackAndDisplay(vpDot2 dot[], const unsigned int &n, - vpImage &I, - std::vector &cogs, - vpImagePoint *cogStar = NULL); + static void trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage &I, + std::vector &cogs, vpImagePoint *cogStar = NULL); public: double m00; /*!< Considering the general distribution moments for \f$ N \f$ @@ -432,26 +414,19 @@ class VISP_EXPORT vpDot2 : public vpTracker */ private: - virtual bool isValid(const vpImage &I, - const vpDot2 &wantedDot); + virtual bool isValid(const vpImage &I, const vpDot2 &wantedDot); - virtual bool hasGoodLevel(const vpImage &I, - const unsigned int &u, - const unsigned int &v) const; - virtual bool hasReverseLevel(const vpImage &I, - const unsigned int &u, - const unsigned int &v) const; + virtual bool hasGoodLevel(const vpImage &I, const unsigned int &u, const unsigned int &v) const; + virtual bool hasReverseLevel(const vpImage &I, const unsigned int &u, const unsigned int &v) const; virtual vpDot2 *getInstance(); void init(); - bool computeParameters(const vpImage &I, - const double &u = -1.0, const double &v = -1.0); + bool computeParameters(const vpImage &I, const double &u = -1.0, const double &v = -1.0); - bool findFirstBorder(const vpImage &I, const unsigned int &u, - const unsigned int &v, unsigned int &border_u, - unsigned int &border_v); + bool findFirstBorder(const vpImage &I, const unsigned int &u, const unsigned int &v, + unsigned int &border_u, unsigned int &border_v); void computeMeanGrayLevel(const vpImage &I); /*! @@ -473,26 +448,19 @@ class VISP_EXPORT vpDot2 : public vpTracker */ unsigned int getFirstBorder_v() const { return this->firstBorder_v; } - bool computeFreemanChainElement(const vpImage &I, - const unsigned int &u, - const unsigned int &v, + bool computeFreemanChainElement(const vpImage &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); + 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 &I) const; - bool isInImage(const vpImage &I, - const vpImagePoint &ip) const; + bool isInImage(const vpImage &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 &I, int u, int v, unsigned int w, - unsigned int h); + void setArea(const vpImage &I, int u, int v, unsigned int w, unsigned int h); void setArea(const vpImage &I); void setArea(const vpRect &a); @@ -539,15 +507,11 @@ class VISP_EXPORT vpDot2 : public vpTracker // Static funtions public: - static void display(const vpImage &I, - const vpImagePoint &cog, - const std::list &edges_list, - vpColor color = vpColor::red, - unsigned int thickness = 1); - static void display(const vpImage &I, const vpImagePoint &cog, - const std::list &edges_list, - vpColor color = vpColor::red, + static void display(const vpImage &I, const vpImagePoint &cog, + const std::list &edges_list, vpColor color = vpColor::red, unsigned int thickness = 1); + static void display(const vpImage &I, const vpImagePoint &cog, const std::list &edges_list, + vpColor color = vpColor::red, unsigned int thickness = 1); }; #endif diff --git a/modules/tracker/blob/src/dots/vpDot.cpp b/modules/tracker/blob/src/dots/vpDot.cpp index a05173f015..ff97114e9a 100644 --- a/modules/tracker/blob/src/dots/vpDot.cpp +++ b/modules/tracker/blob/src/dots/vpDot.cpp @@ -91,13 +91,10 @@ void vpDot::init() } vpDot::vpDot() - : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), - mu02(0.), ip_connexities_list(), ip_edges_list(), - connexityType(CONNEXITY_4), cog(), u_min(0), u_max(0), v_min(0), v_max(0), - graphics(false), thickness(1), maxDotSizePercentage(0.25), - gray_level_out(0), mean_gray_level(0), gray_level_min(128), - gray_level_max(255), grayLevelPrecision(0.85), gamma(1.5), - compute_moment(false), nbMaxPoint(0) + : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), 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) { } @@ -107,13 +104,10 @@ vpDot::vpDot() \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(ip), u_min(0), u_max(0), v_min(0), - v_max(0), graphics(false), thickness(1), maxDotSizePercentage(0.25), - gray_level_out(0), mean_gray_level(0), gray_level_min(128), - gray_level_max(255), grayLevelPrecision(0.85), gamma(1.5), - compute_moment(false), nbMaxPoint(0) + : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), ip_connexities_list(), + ip_edges_list(), connexityType(CONNEXITY_4), cog(ip), u_min(0), u_max(0), v_min(0), v_max(0), graphics(false), + thickness(1), maxDotSizePercentage(0.25), gray_level_out(0), mean_gray_level(0), gray_level_min(128), + gray_level_max(255), grayLevelPrecision(0.85), gamma(1.5), compute_moment(false), nbMaxPoint(0) { } @@ -121,13 +115,10 @@ vpDot::vpDot(const vpImagePoint &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) + : 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; } @@ -199,8 +190,7 @@ void vpDot::setGrayLevelOut() 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")); + throw(vpTrackingException(vpTrackingException::initializationError, "Unable to choose a good \"out\" level")); } gray_level_out = static_cast(gray_level_max + 1u); } @@ -223,8 +213,7 @@ void vpDot::setGrayLevelOut() \sa setGrayLevelOut() */ -bool vpDot::connexe(const vpImage &I, unsigned int u, - unsigned int v, double &mean_value, double &u_cog, +bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog, double &v_cog, double &n) { std::vector checkTab(I.getWidth() * I.getHeight(), false); @@ -247,8 +236,7 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, \sa setGrayLevelOut() */ -bool vpDot::connexe(const vpImage &I, unsigned int u, - unsigned int v, double &mean_value, double &u_cog, +bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog, double &v_cog, double &n, std::vector &checkTab) { @@ -280,17 +268,15 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, if (n > nbMaxPoint) { // vpERROR_TRACE("Too many point %lf (%lf%% of image size). " // "This threshold can be modified using the - //setMaxDotSize() " "method.", n, n / + // 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)); + // 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 @@ -457,8 +443,7 @@ void vpDot::COG(const vpImage &I, double &u, double &v) } #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)) { + if (!connexe(I, (unsigned int)u, (unsigned int)v, mean_gray_level, u_cog, v_cog, npoint)) { bool sol = false; unsigned int right = 1; @@ -478,8 +463,7 @@ void vpDot::COG(const vpImage &I, double &u, double &v) 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)) { + 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_; @@ -497,8 +481,7 @@ void vpDot::COG(const vpImage &I, double &u, double &v) this->mean_gray_level = 0; - if (connexe(I, (unsigned int)(u_), (unsigned int)(v_ + k), - mean_gray_level, u_cog, v_cog, npoint)) { + 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; @@ -516,8 +499,7 @@ void vpDot::COG(const vpImage &I, double &u, double &v) this->mean_gray_level = 0; - if (connexe(I, (unsigned int)(u_ - k), (unsigned int)(v_), - mean_gray_level, u_cog, v_cog, npoint)) { + 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_; @@ -535,8 +517,7 @@ void vpDot::COG(const vpImage &I, double &u, double &v) this->mean_gray_level = 0; - if (connexe(I, (unsigned int)(u_), (unsigned int)(v_ - k), - mean_gray_level, u_cog, v_cog, npoint)) { + 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; @@ -548,8 +529,7 @@ void vpDot::COG(const vpImage &I, double &u, double &v) if (sol == false) { // vpERROR_TRACE("Dot has been lost") ; - throw(vpTrackingException(vpTrackingException::featureLostError, - "Dot has been lost")); + throw(vpTrackingException(vpTrackingException::featureLostError, "Dot has been lost")); } } @@ -575,13 +555,11 @@ void vpDot::COG(const vpImage &I, double &u, double &v) if (Ip - (1 - grayLevelPrecision) < 0) { gray_level_min = 0; } else { - gray_level_min = - (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); + 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)); + gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); if (gray_level_max > 255) gray_level_max = 255; @@ -590,8 +568,7 @@ void vpDot::COG(const vpImage &I, double &u, double &v) if (npoint < 5) { // vpERROR_TRACE("Dot to small") ; - throw(vpTrackingException(vpTrackingException::featureLostError, - "Dot to small")); + throw(vpTrackingException(vpTrackingException::featureLostError, "Dot to small")); } if (npoint > nbMaxPoint) { @@ -605,8 +582,7 @@ void vpDot::COG(const vpImage &I, double &u, double &v) "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)); + npoint, npoint / (I.getWidth() * I.getHeight()), nbMaxPoint, maxDotSizePercentage)); } } @@ -627,8 +603,7 @@ 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); + "Value should be in ]0:1]. Value will be set to %lf.", percentage, maxDotSizePercentage); } else { maxDotSizePercentage = percentage; } @@ -670,13 +645,11 @@ void vpDot::initTracking(const vpImage &I) if (Ip - (1 - grayLevelPrecision) < 0) { gray_level_min = 0; } else { - gray_level_min = - (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); + 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)); + gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); if (gray_level_max > 255) gray_level_max = 255; @@ -710,8 +683,7 @@ void vpDot::initTracking(const vpImage &I) \sa track() */ -void vpDot::initTracking(const vpImage &I, - const vpImagePoint &ip) +void vpDot::initTracking(const vpImage &I, const vpImagePoint &ip) { cog = ip; @@ -724,13 +696,11 @@ void vpDot::initTracking(const vpImage &I, if (Ip - (1 - grayLevelPrecision) < 0) { gray_level_min = 0; } else { - gray_level_min = - (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); + 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)); + gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); if (gray_level_max > 255) gray_level_max = 255; try { @@ -767,8 +737,7 @@ void vpDot::initTracking(const vpImage &I, \sa track(), getCog() */ -void vpDot::initTracking(const vpImage &I, - const vpImagePoint &ip, unsigned int level_min, +void vpDot::initTracking(const vpImage &I, const vpImagePoint &ip, unsigned int level_min, unsigned int level_max) { @@ -818,8 +787,7 @@ void vpDot::track(const vpImage &I) 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::displayCross(I, this->cog, 3 * thickness + 8, vpColor::red, thickness); } } catch (vpException &e) { @@ -855,8 +823,7 @@ void vpDot::track(const vpImage &I, vpImagePoint &ip) \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 &I, vpColor color, - unsigned int thick) const +void vpDot::display(const vpImage &I, vpColor color, unsigned int thick) const { vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thick); std::list::const_iterator it; @@ -910,9 +877,8 @@ void vpDot::setGrayLevelPrecision(const double &precision) \param thickness : Thickness of the dot. */ -void vpDot::display(const vpImage &I, const vpImagePoint &cog, - const std::list &edges_list, vpColor color, - unsigned int thickness) +void vpDot::display(const vpImage &I, const vpImagePoint &cog, const std::list &edges_list, + vpColor color, unsigned int thickness) { vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thickness); std::list::const_iterator it; @@ -936,9 +902,8 @@ void vpDot::display(const vpImage &I, const vpImagePoint &cog, \param thickness : Thickness of the dot. */ -void vpDot::display(const vpImage &I, const vpImagePoint &cog, - const std::list &edges_list, vpColor color, - unsigned int thickness) +void vpDot::display(const vpImage &I, const vpImagePoint &cog, const std::list &edges_list, + vpColor color, unsigned int thickness) { vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thickness); std::list::const_iterator it; @@ -953,7 +918,4 @@ void vpDot::display(const vpImage &I, const vpImagePoint &cog, 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() << ")"); -}; +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 index ca5078a6a4..4800f8d3f5 100644 --- a/modules/tracker/blob/src/dots/vpDot2.cpp +++ b/modules/tracker/blob/src/dots/vpDot2.cpp @@ -102,14 +102,11 @@ void vpDot2::init() Default constructor. Just do basic default initialization. */ vpDot2::vpDot2() - : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), - mu02(0.), cog(), width(0), height(0), surface(0), gray_level_min(128), - gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), - gamma(1.5), sizePrecision(0.65), ellipsoidShapePrecision(0.65), - maxSizeSearchDistancePrecision(0.65), allowedBadPointsPercentage_(0.), - area(), direction_list(), ip_edges_list(), compute_moment(false), - graphics(false), thickness(1), bbox_u_min(0), bbox_u_max(0), - bbox_v_min(0), bbox_v_max(0), firstBorder_u(0), firstBorder_v() + : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), 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() { } @@ -122,14 +119,11 @@ vpDot2::vpDot2() */ vpDot2::vpDot2(const vpImagePoint &ip) - : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), - mu02(0.), cog(ip), width(0), height(0), surface(0), gray_level_min(128), - gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), - gamma(1.5), sizePrecision(0.65), ellipsoidShapePrecision(0.65), - maxSizeSearchDistancePrecision(0.65), allowedBadPointsPercentage_(0.), - area(), direction_list(), ip_edges_list(), compute_moment(false), - graphics(false), thickness(1), bbox_u_min(0), bbox_u_max(0), - bbox_v_min(0), bbox_v_max(0), firstBorder_u(0), firstBorder_v() + : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), cog(ip), width(0), height(0), + surface(0), gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5), + sizePrecision(0.65), ellipsoidShapePrecision(0.65), maxSizeSearchDistancePrecision(0.65), + allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false), graphics(false), + thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0), firstBorder_u(0), firstBorder_v() { } @@ -137,14 +131,11 @@ vpDot2::vpDot2(const vpImagePoint &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), + : 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; @@ -218,8 +209,7 @@ vpDot2::~vpDot2() {} \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 &I, vpColor color, - unsigned int t) const +void vpDot2::display(const vpImage &I, vpColor color, unsigned int t) const { vpDisplay::displayCross(I, cog, 3 * t + 8, color, t); std::list::const_iterator it; @@ -273,13 +263,11 @@ void vpDot2::initTracking(const vpImage &I, unsigned int size) if (Ip - (1 - grayLevelPrecision) < 0) { gray_level_min = 0; } else { - gray_level_min = - (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); + 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)); + gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); if (gray_level_max > 255) gray_level_max = 255; @@ -321,8 +309,7 @@ void vpDot2::initTracking(const vpImage &I, unsigned int size) This is the default case. To track a non ellipsoid shape use setEllipsoidShapePrecision(0). */ -void vpDot2::initTracking(const vpImage &I, - const vpImagePoint &ip, unsigned int size) +void vpDot2::initTracking(const vpImage &I, const vpImagePoint &ip, unsigned int size) { cog = ip; @@ -334,13 +321,11 @@ void vpDot2::initTracking(const vpImage &I, if (Ip - (1 - grayLevelPrecision) < 0) { gray_level_min = 0; } else { - gray_level_min = - (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); + 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)); + gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); if (gray_level_max > 255) gray_level_max = 255; @@ -394,8 +379,7 @@ void vpDot2::initTracking(const vpImage &I, \sa track(), getCog() */ -void vpDot2::initTracking(const vpImage &I, - const vpImagePoint &ip, unsigned int gray_lvl_min, +void vpDot2::initTracking(const vpImage &I, const vpImagePoint &ip, unsigned int gray_lvl_min, unsigned int gray_lvl_max, unsigned int size) { cog = ip; @@ -506,16 +490,14 @@ void vpDot2::track(const vpImage &I) } std::list candidates; searchDotsInArea(I, (int)(this->cog.get_u() - searchWindowWidth / 2.0), - (int)(this->cog.get_v() - searchWindowHeight / 2.0), - (unsigned int)searchWindowWidth, + (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")); + throw(vpTrackingException(vpTrackingException::featureLostError, "No dot was found")); } // otherwise we've got our dot, update this dot's parameters @@ -562,9 +544,8 @@ void vpDot2::track(const vpImage &I) // 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")); + throw(vpTrackingException(vpTrackingException::featureLostError, + "The center of gravity of the dot is not in the image")); } // Get dots center of gravity @@ -579,13 +560,11 @@ void vpDot2::track(const vpImage &I) if (Ip - (1 - grayLevelPrecision) < 0) { gray_level_min = 0; } else { - gray_level_min = - (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); + 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)); + gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); if (gray_level_max > 255) gray_level_max = 255; @@ -593,8 +572,7 @@ void vpDot2::track(const vpImage &I) 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::displayCross(I, this->cog, 3 * thickness + 8, vpColor::red, thickness); // vpDisplay::flush(I); } } @@ -669,10 +647,7 @@ double vpDot2::getSizePrecision() const { return sizePrecision; } \sa setEllipsoidShapePrecision() */ -double vpDot2::getEllipsoidShapePrecision() const -{ - return ellipsoidShapePrecision; -} +double vpDot2::getEllipsoidShapePrecision() const { return ellipsoidShapePrecision; } /*! Return the precision of the search maximum distance to get the starting @@ -680,10 +655,7 @@ double vpDot2::getEllipsoidShapePrecision() const [0.05,1]. 1 means full precision, whereas values close to 0 show a very bad precision. */ -double vpDot2::getMaxSizeSearchDistancePrecision() const -{ - return maxSizeSearchDistancePrecision; -} +double vpDot2::getMaxSizeSearchDistancePrecision() const { return maxSizeSearchDistancePrecision; } /*! Return the distance between the two center of dots. @@ -811,8 +783,10 @@ void vpDot2::setSizePrecision(const double &precision) 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 + shapes. + \code + vpDot2 dot; + dot.setEllipsoidShapePrecision(0.); // to track a blob without any constraint on the shape ... dot.track(); \endcode @@ -866,10 +840,7 @@ void vpDot2::setMaxSizeSearchDistancePrecision(const double &precision) \param I : Image. */ -void vpDot2::setArea(const vpImage &I) -{ - setArea(I, 0, 0, I.getWidth(), I.getHeight()); -} +void vpDot2::setArea(const vpImage &I) { setArea(I, 0, 0, I.getWidth(), I.getHeight()); } /*! @@ -883,8 +854,7 @@ void vpDot2::setArea(const vpImage &I) \param h : Area height. */ -void vpDot2::setArea(const vpImage &I, int u, int v, - unsigned int w, unsigned int h) +void vpDot2::setArea(const vpImage &I, int u, int v, unsigned int w, unsigned int h) { unsigned int image_w = I.getWidth(); unsigned int image_h = I.getHeight(); @@ -969,8 +939,7 @@ void vpDot2::setArea(const vpRect &a) { area = a; } \sa searchDotsInArea(vpImage&, int, int, unsigned int, unsigned int, std::list &) */ -void vpDot2::searchDotsInArea(const vpImage &I, - std::list &niceDots) +void vpDot2::searchDotsInArea(const vpImage &I, std::list &niceDots) { searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), niceDots); } @@ -997,10 +966,8 @@ void vpDot2::searchDotsInArea(const vpImage &I, \sa searchDotsInArea(vpImage& I, std::list &) */ -void vpDot2::searchDotsInArea(const vpImage &I, int area_u, - int area_v, unsigned int area_w, - unsigned int area_h, - std::list &niceDots) +void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int area_v, unsigned int area_w, + unsigned int area_h, std::list &niceDots) { // clear the list of nice dots @@ -1065,8 +1032,7 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, 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)) { + 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; } @@ -1093,12 +1059,9 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, 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::const_iterator it_edges = - ip_edges_list.begin(); + 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::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 @@ -1106,12 +1069,10 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, 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())) * + vpMath::maximum(std::fabs((double)border_u), std::fabs(cogBadDot.get_u())) * std::numeric_limits::epsilon()) && (std::fabs(v - cogBadDot.get_v()) <= - vpMath::maximum(std::fabs((double)v), - std::fabs(cogBadDot.get_v())) * + vpMath::maximum(std::fabs((double)v), std::fabs(cogBadDot.get_v())) * std::numeric_limits::epsilon())) { good_germ = false; } @@ -1176,8 +1137,7 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, 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); + double thisDist = sqrt(thisDiff_u * thisDiff_u + thisDiff_v * thisDiff_v); bool stopLoop = false; itnice = niceDots.begin(); @@ -1203,8 +1163,7 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, 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); + 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, @@ -1270,12 +1229,9 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) // if ( (wantedDot.getWidth() != 0) // && (wantedDot.getHeight() != 0) // && (wantedDot.getArea() != 0) ) - if ((std::fabs(wantedDot.getWidth()) > - std::numeric_limits::epsilon()) && - (std::fabs(wantedDot.getHeight()) > - std::numeric_limits::epsilon()) && - (std::fabs(wantedDot.getArea()) > - std::numeric_limits::epsilon())) + if ((std::fabs(wantedDot.getWidth()) > std::numeric_limits::epsilon()) && + (std::fabs(wantedDot.getHeight()) > std::numeric_limits::epsilon()) && + (std::fabs(wantedDot.getArea()) > std::numeric_limits::epsilon())) // if (size_precision!=0){ { if (std::fabs(size_precision) > std::numeric_limits::epsilon()) { @@ -1283,85 +1239,61 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) #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; + << "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; + << "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()); + 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()); + 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()); + 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()); + 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(), + 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()); + 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), + 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()); + if ((wantedDot.getArea() * (size_precision * size_precision) - epsilon < getArea()) == false) { + vpDEBUG_TRACE(3, "Bad surface > for dot (%g, %g)", cog.get_u(), cog.get_v()); #ifdef DEBUG printf("Bad surface %g > %g for dot (%g, %g)\n", - wantedDot.getArea() * (size_precision * size_precision) - - epsilon, - getArea(), cog.get_u(), cog.get_v()); + wantedDot.getArea() * (size_precision * size_precision) - epsilon, getArea(), 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()); + if ((getArea() < wantedDot.getArea() / (size_precision * size_precision + epsilon)) == false) { + vpDEBUG_TRACE(3, "Bad surface > for dot (%g, %g)", cog.get_u(), cog.get_v()); #ifdef DEBUG printf("Bad surface %g < %g for dot (%g, %g)\n", getArea(), - wantedDot.getArea() / - (size_precision * size_precision + epsilon), - cog.get_u(), cog.get_v()); + wantedDot.getArea() / (size_precision * size_precision + epsilon), cog.get_u(), cog.get_v()); #endif return false; } @@ -1372,17 +1304,13 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) // 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_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_); + 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::epsilon() && - compute_moment) { + if (std::fabs(ellipsoidShape_precision) > std::numeric_limits::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 @@ -1403,12 +1331,9 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) 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)); + 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 @@ -1423,10 +1348,8 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) 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))); + 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); @@ -1452,8 +1375,7 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) } 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); + printf("Inner ellipse has %d bad points. Max allowed is %d\n", nb_bad_points, nb_max_bad_points); #endif return false; } @@ -1465,18 +1387,16 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) 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))); + 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, (int)v, (int)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()) { + if ((double)u < area.getLeft() || (double)u > area.getRight() || (double)v < area.getTop() || + (double)v > area.getBottom()) { continue; } if (!this->hasReverseLevel(I, u, v)) { @@ -1502,8 +1422,7 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) } 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); + printf("Outside ellipse has %d bad points. Max allowed is %d\n", nb_bad_points, nb_max_bad_points); #endif return false; } @@ -1529,8 +1448,7 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) \sa setGrayLevelMin(), setGrayLevelMax() */ -bool vpDot2::hasGoodLevel(const vpImage &I, - const unsigned int &u, const unsigned int &v) const +bool vpDot2::hasGoodLevel(const vpImage &I, const unsigned int &u, const unsigned int &v) const { if (!isInArea(u, v)) return false; @@ -1554,9 +1472,7 @@ bool vpDot2::hasGoodLevel(const vpImage &I, \return true if it is so, and false otherwise. */ -bool vpDot2::hasReverseLevel(const vpImage &I, - const unsigned int &u, - const unsigned int &v) const +bool vpDot2::hasReverseLevel(const vpImage &I, const unsigned int &u, const unsigned int &v) const { if (!isInArea(u, v)) @@ -1594,10 +1510,7 @@ vpDot2 *vpDot2::getInstance() { return new vpDot2(); } - 6 : down - 7 : down right */ -void vpDot2::getFreemanChain(std::list &freeman_chain) const -{ - freeman_chain = direction_list; -} +void vpDot2::getFreemanChain(std::list &freeman_chain) const { freeman_chain = direction_list; } /****************************************************************************** * @@ -1636,8 +1549,7 @@ void vpDot2::getFreemanChain(std::list &freeman_chain) const \sa getFirstBorder_u(), getFirstBorder_v() */ -bool vpDot2::computeParameters(const vpImage &I, - const double &_u, const double &_v) +bool vpDot2::computeParameters(const vpImage &I, const double &_u, const double &_v) { direction_list.clear(); ip_edges_list.clear(); @@ -1647,15 +1559,13 @@ bool vpDot2::computeParameters(const vpImage &I, // if u has default value, set it to the actual center value // if( est_u == -1.0 ) - if (std::fabs(est_u + 1.0) <= vpMath::maximum(std::fabs(est_u), 1.) * - std::numeric_limits::epsilon()) { + if (std::fabs(est_u + 1.0) <= vpMath::maximum(std::fabs(est_u), 1.) * std::numeric_limits::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::epsilon()) { + if (std::fabs(est_v + 1.0) <= vpMath::maximum(std::fabs(est_v), 1.) * std::numeric_limits::epsilon()) { est_v = this->cog.get_v(); } @@ -1677,33 +1587,28 @@ bool vpDot2::computeParameters(const vpImage &I, // 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); + 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)) { + 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); + 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); + 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); + vpDEBUG_TRACE(3, "Border pixel coordinates (%d, %d) of the dot are not in the area", this->firstBorder_u, + this->firstBorder_v); return false; } @@ -1791,18 +1696,15 @@ bool vpDot2::computeParameters(const vpImage &I, 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); + 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 || + } while ((getFirstBorder_u() != (unsigned int)border_u || getFirstBorder_v() != (unsigned int)border_v || firstDir != dir) && isInArea((unsigned int)border_u, (unsigned int)border_v)); @@ -1816,10 +1718,8 @@ bool vpDot2::computeParameters(const vpImage &I, // detected. Return an error tracking. // if( m00 == 0 || m00 == 1 ) if (std::fabs(m00) <= std::numeric_limits::epsilon() || - std::fabs(m00 - 1.) <= vpMath::maximum(std::fabs(m00), 1.) * - std::numeric_limits::epsilon()) { - vpDEBUG_TRACE( - 3, "The center of gravity of the dot wasn't properly detected"); + std::fabs(m00 - 1.) <= vpMath::maximum(std::fabs(m00), 1.) * std::numeric_limits::epsilon()) { + vpDEBUG_TRACE(3, "The center of gravity of the dot wasn't properly detected"); return false; } else // compute the center { @@ -1869,8 +1769,7 @@ crossed the dot on more than the max possible width. Return true if success. \sa computeParameters() */ -bool vpDot2::findFirstBorder(const vpImage &I, - const unsigned int &u, const unsigned int &v, +bool vpDot2::findFirstBorder(const vpImage &I, const unsigned int &u, const unsigned int &v, unsigned int &border_u, unsigned int &border_v) { // find the border @@ -1884,17 +1783,13 @@ bool vpDot2::findFirstBorder(const vpImage &I, double epsilon = 0.001; #ifdef DEBUG - std::cout << "gray level: " << gray_level_min << " " << gray_level_max - << std::endl; + 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()*/) { + 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)) { + 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", @@ -1902,8 +1797,7 @@ bool vpDot2::findFirstBorder(const vpImage &I, return false; } #ifdef DEBUG - vpDisplay::displayPoint(I, (int)border_v, (int)border_u + 1, - vpColor::green); + vpDisplay::displayPoint(I, (int)border_v, (int)border_u + 1, vpColor::green); vpDisplay::flush(I); #endif @@ -1930,9 +1824,7 @@ bool vpDot2::findFirstBorder(const vpImage &I, \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 &I, - const unsigned int &u, - const unsigned int &v, +bool vpDot2::computeFreemanChainElement(const vpImage &I, const unsigned int &u, const unsigned int &v, unsigned int &element) { @@ -2044,10 +1936,8 @@ bool vpDot2::computeFreemanChainElement(const vpImage &I, */ -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) +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; @@ -2085,12 +1975,9 @@ void vpDot2::computeFreemanParameters(const int &u_p, const int &v_p, 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); + 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; @@ -2114,12 +2001,9 @@ void vpDot2::computeFreemanParameters(const int &u_p, const int &v_p, 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); + 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; @@ -2143,12 +2027,9 @@ void vpDot2::computeFreemanParameters(const int &u_p, const int &v_p, 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); + 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; @@ -2172,12 +2053,9 @@ void vpDot2::computeFreemanParameters(const int &u_p, const int &v_p, 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); + 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; } @@ -2196,8 +2074,7 @@ void vpDot2::computeFreemanParameters(const int &u_p, const int &v_p, 6=up and 7. */ -void vpDot2::updateFreemanPosition(unsigned int &u, unsigned int &v, - const unsigned int &dir) +void vpDot2::updateFreemanPosition(unsigned int &u, unsigned int &v, const unsigned int &dir) { switch (dir) { case 0: @@ -2242,10 +2119,7 @@ void vpDot2::updateFreemanPosition(unsigned int &u, unsigned int &v, \return true if the pixel of coordinates (posI, posJ) is in the image and false otherwise. */ -bool vpDot2::isInImage(const vpImage &I) const -{ - return isInImage(I, cog); -} +bool vpDot2::isInImage(const vpImage &I) const { return isInImage(I, cog); } /*! @@ -2258,8 +2132,7 @@ bool vpDot2::isInImage(const vpImage &I) const \return true if the image point \e ip is in the image and false otherwise. */ -bool vpDot2::isInImage(const vpImage &I, - const vpImagePoint &ip) const +bool vpDot2::isInImage(const vpImage &I, const vpImagePoint &ip) const { unsigned int h = I.getHeight(); unsigned int w = I.getWidth(); @@ -2316,11 +2189,8 @@ void vpDot2::getGridSize(unsigned int &gridWidth, unsigned int &gridHeight) // 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)); + gridWidth = (unsigned int)(getWidth() * getMaxSizeSearchDistancePrecision() / sqrt(2.)); + gridHeight = (unsigned int)(getHeight() * getMaxSizeSearchDistancePrecision() / sqrt(2.0)); if (gridWidth == 0) gridWidth = 1; @@ -2348,24 +2218,21 @@ void vpDot2::computeMeanGrayLevel(const vpImage &I) 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++) { + 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++) { + 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 + 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)) { @@ -2379,10 +2246,8 @@ void vpDot2::computeMeanGrayLevel(const vpImage &I) 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()) { + 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++; } @@ -2400,10 +2265,8 @@ void vpDot2::computeMeanGrayLevel(const vpImage &I) } 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()) { + 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++; } @@ -2412,8 +2275,7 @@ void vpDot2::computeMeanGrayLevel(const vpImage &I) if (nb_pixels == 0) { // should never happen - throw(vpTrackingException(vpTrackingException::notEnoughPointError, - "No point was found")); + throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No point was found")); } else { mean_gray_level = sum_value / nb_pixels; } @@ -2437,10 +2299,8 @@ void vpDot2::computeMeanGrayLevel(const vpImage &I) \return an nx2 matrix with the coordinates of the dots */ -vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, - const std::string &dotFile, - vpImage &I, vpColor col, - bool trackDot) +vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage &I, + vpColor col, bool trackDot) { vpMatrix Cogs(n, 2); vpImagePoint cog; @@ -2448,14 +2308,12 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, bool fromFile = vpIoTools::checkFilename(dotFile.c_str()); if (fromFile) { vpMatrix::loadMatrix(dotFile, Cogs); - std::cout << Cogs.getRows() << " dots loaded from file " << dotFile - << std::endl; + 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; + std::cout << "Dot file has a wrong number of dots : redefining them" << std::endl; fromFile = false; } @@ -2480,8 +2338,7 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, // check that dots are far away ones from the other for (i = 0; i < n && fromFile; ++i) { - double d = sqrt(vpMath::sqr(dot[i].getHeight()) + - vpMath::sqr(dot[i].getWidth())); + double 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) { @@ -2495,9 +2352,7 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, vpDisplay::display(I); vpDisplay::flush(I); - std::cout << "Click on the " << n - << " dots clockwise starting from upper/left dot..." - << std::endl; + 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); @@ -2516,8 +2371,7 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, if (!fromFile && (dotFile != "")) { vpMatrix::saveMatrix(dotFile, Cogs); - std::cout << Cogs.getRows() << " dots written to file " << dotFile - << std::endl; + std::cout << Cogs.getRows() << " dots written to file " << dotFile << std::endl; } // back to non graphic mode @@ -2543,10 +2397,8 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, vpImagePoint indicating the desired position (default NULL), will be displayed in red */ -void vpDot2::trackAndDisplay(vpDot2 dot[], const unsigned int &n, - vpImage &I, - std::vector &cogs, - vpImagePoint *cogStar) +void vpDot2::trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage &I, + std::vector &cogs, vpImagePoint *cogStar) { unsigned int i; // tracking @@ -2584,8 +2436,7 @@ void vpDot2::trackAndDisplay(vpDot2 dot[], const unsigned int &n, \param thickness : Thickness of the dot. */ void vpDot2::display(const vpImage &I, const vpImagePoint &cog, - const std::list &edges_list, vpColor color, - unsigned int thickness) + const std::list &edges_list, vpColor color, unsigned int thickness) { vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thickness); std::list::const_iterator it; @@ -2609,9 +2460,8 @@ void vpDot2::display(const vpImage &I, const vpImagePoint &cog, \param thickness : Thickness of the dot. */ -void vpDot2::display(const vpImage &I, const vpImagePoint &cog, - const std::list &edges_list, vpColor color, - unsigned int thickness) +void vpDot2::display(const vpImage &I, const vpImagePoint &cog, const std::list &edges_list, + vpColor color, unsigned int thickness) { vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thickness); std::list::const_iterator it; @@ -2626,7 +2476,4 @@ void vpDot2::display(const vpImage &I, const vpImagePoint &cog, 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() << ")"); -} +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 index d008d448d6..cccd131251 100644 --- a/modules/tracker/blob/test/tracking/testTrackDot.cpp +++ b/modules/tracker/blob/test/tracking/testTrackDot.cpp @@ -43,8 +43,7 @@ #include #include #include -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || \ - defined(VISP_HAVE_GDI)) +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) #include #include @@ -69,8 +68,7 @@ // 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); +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); /*! @@ -125,8 +123,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -185,8 +182,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == - false) { + if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -200,8 +196,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -210,13 +205,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -249,8 +241,7 @@ int main(int argc, const char **argv) 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -314,9 +305,6 @@ int main(int argc, const char **argv) } } #else -int main() -{ - vpERROR_TRACE("You do not have X11, GTK or GDI display functionalities..."); -} +int main() { vpERROR_TRACE("You do not have X11, GTK or GDI display functionalities..."); } #endif diff --git a/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h b/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h index 095890a2db..2ea14d3d1b 100644 --- a/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h +++ b/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h @@ -86,26 +86,16 @@ class VISP_EXPORT vpKltOpencv void addFeature(const long &id, const float &x, const float &y); void addFeature(const cv::Point2f &f); - void display(const vpImage &I, - const vpColor &color = vpColor::red, - unsigned int thickness = 1); - static void display(const vpImage &I, - const std::vector &features, - const vpColor &color = vpColor::green, + void display(const vpImage &I, const vpColor &color = vpColor::red, unsigned int thickness = 1); + static void display(const vpImage &I, const std::vector &features, + const vpColor &color = vpColor::green, unsigned int thickness = 1); + static void display(const vpImage &I, const std::vector &features, + const vpColor &color = vpColor::green, unsigned int thickness = 1); + static void display(const vpImage &I, const std::vector &features, + const std::vector &featuresid, const vpColor &color = vpColor::green, unsigned int thickness = 1); - static void display(const vpImage &I, - const std::vector &features, - const vpColor &color = vpColor::green, - unsigned int thickness = 1); - static void display(const vpImage &I, - const std::vector &features, - const std::vector &featuresid, - const vpColor &color = vpColor::green, - unsigned int thickness = 1); - static void display(const vpImage &I, - const std::vector &features, - const std::vector &featuresid, - const vpColor &color = vpColor::green, + static void display(const vpImage &I, const std::vector &features, + const std::vector &featuresid, const vpColor &color = vpColor::green, unsigned int thickness = 1); //! Get the size of the averaging block used to track the features. @@ -146,16 +136,14 @@ class VISP_EXPORT vpKltOpencv void initTracking(const cv::Mat &I, const cv::Mat &mask = cv::Mat()); void initTracking(const cv::Mat &I, const std::vector &pts); - void initTracking(const cv::Mat &I, const std::vector &pts, - const std::vector &ids); + void initTracking(const cv::Mat &I, const std::vector &pts, const std::vector &ids); vpKltOpencv &operator=(const vpKltOpencv ©); void track(const cv::Mat &I); void setBlockSize(const int blockSize); void setHarrisFreeParameter(double harris_k); void setInitialGuess(const std::vector &guess_pts); - void setInitialGuess(const std::vector &init_pts, - const std::vector &guess_pts, + void setInitialGuess(const std::vector &init_pts, const std::vector &guess_pts, const std::vector &fid); void setMaxFeatures(const int maxCount); void setMinDistance(double minDistance); @@ -171,9 +159,8 @@ class VISP_EXPORT vpKltOpencv protected: cv::Mat m_gray, m_prevGray; - std::vector - m_points[2]; //!< Previous [0] and current [1] keypoint location - std::vector m_points_id; //!< Keypoint id + std::vector m_points[2]; //!< Previous [0] and current [1] keypoint location + std::vector m_points_id; //!< Keypoint id int m_maxCount; cv::TermCriteria m_termcrit; int m_winSize; @@ -241,21 +228,20 @@ class VISP_EXPORT vpKltOpencv 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 + 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 @@ -279,8 +265,8 @@ class VISP_EXPORT vpKltOpencv 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 + char *status; // Result of the tracker for every features : 0 = lost, 1 = + // found // EVENT FUNCTION POINTERS funcevent OnInitialize; @@ -303,8 +289,7 @@ class VISP_EXPORT vpKltOpencv void addFeature(const int &id, const float &x, const float &y); // Draw the tracked features on the given image - void display(const vpImage &I, vpColor color = vpColor::red, - unsigned int thickness = 1); + void display(const vpImage &I, vpColor color = vpColor::red, unsigned int thickness = 1); //! Get the block size int getBlockSize() const { return block_size; } @@ -340,8 +325,7 @@ class VISP_EXPORT vpKltOpencv // 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); + void initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size); vpKltOpencv &operator=(const vpKltOpencv ©); // Track ! void track(const IplImage *I); @@ -374,8 +358,7 @@ class VISP_EXPORT vpKltOpencv harris_free_parameter = input; } void setInitialGuess(CvPoint2D32f **guess_pts); - void setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, - long *fid, int size); + 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) @@ -452,25 +435,15 @@ class VISP_EXPORT vpKltOpencv // Static Functions public: - static void display(const vpImage &I, - const CvPoint2D32f *features_list, - const int &nbFeatures, vpColor color = vpColor::green, - unsigned int thickness = 1); - static void display(const vpImage &I, - const CvPoint2D32f *features_list, - const int &nbFeatures, vpColor color = vpColor::green, - unsigned int thickness = 1); - - static void display(const vpImage &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 &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 &I, const CvPoint2D32f *features_list, const int &nbFeatures, + vpColor color = vpColor::green, unsigned int thickness = 1); + static void display(const vpImage &I, const CvPoint2D32f *features_list, const int &nbFeatures, + vpColor color = vpColor::green, unsigned int thickness = 1); + + static void display(const vpImage &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 &I, const CvPoint2D32f *features_list, const long *featuresid_list, + const int &nbFeatures, vpColor color = vpColor::green, unsigned int thickness = 1); }; #endif diff --git a/modules/tracker/klt/src/vpKltOpencv.cpp b/modules/tracker/klt/src/vpKltOpencv.cpp index 246fd47b7b..d6e3398d14 100644 --- a/modules/tracker/klt/src/vpKltOpencv.cpp +++ b/modules/tracker/klt/src/vpKltOpencv.cpp @@ -59,25 +59,20 @@ 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_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); + m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.03); } /*! Copy constructor. */ vpKltOpencv::vpKltOpencv(const vpKltOpencv ©) - : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), - m_winSize(10), m_qualityLevel(0.01), m_minDistance(15), - m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), - m_useHarrisDetector(1), m_pyrMaxLevel(3), m_next_points_id(0), - m_initial_guess(false) + : 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; } @@ -133,13 +128,11 @@ void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask) m_points_id.clear(); - cv::goodFeaturesToTrack(m_gray, m_points[1], m_maxCount, m_qualityLevel, - m_minDistance, mask, m_blockSize, false, + cv::goodFeaturesToTrack(m_gray, m_points[1], m_maxCount, m_qualityLevel, m_minDistance, mask, m_blockSize, false, 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); + 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++); @@ -154,8 +147,7 @@ void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask) void vpKltOpencv::track(const cv::Mat &I) { if (m_points[1].size() == 0) - throw vpTrackingException(vpTrackingException::fatalError, - "Not enough key points to track."); + throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track."); std::vector err; int flags = 0; @@ -178,10 +170,8 @@ void vpKltOpencv::track(const cv::Mat &I) std::vector 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); + 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--) { @@ -206,12 +196,10 @@ void vpKltOpencv::track(const cv::Mat &I) \param y : y coordinate. */ -void vpKltOpencv::getFeature(const int &index, long &id, float &x, - float &y) const +void vpKltOpencv::getFeature(const int &index, long &id, float &x, float &y) const { if ((size_t)index >= m_points[1].size()) { - throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", - index)); + throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index)); } x = m_points[1][(size_t)index].x; @@ -226,8 +214,7 @@ void vpKltOpencv::getFeature(const int &index, long &id, float &x, \param color : Color used to display the features. \param thickness : Thickness of the drawings. */ -void vpKltOpencv::display(const vpImage &I, - const vpColor &color, unsigned int thickness) +void vpKltOpencv::display(const vpImage &I, const vpColor &color, unsigned int thickness) { vpKltOpencv::display(I, m_points[1], m_points_id, color, thickness); } @@ -244,8 +231,7 @@ void vpKltOpencv::display(const vpImage &I, \param thickness : Thickness of the points. */ -void vpKltOpencv::display(const vpImage &I, - const std::vector &features, +void vpKltOpencv::display(const vpImage &I, const std::vector &features, const vpColor &color, unsigned int thickness) { vpImagePoint ip; @@ -268,9 +254,8 @@ void vpKltOpencv::display(const vpImage &I, \param thickness : Thickness of the points. */ -void vpKltOpencv::display(const vpImage &I, - const std::vector &features, - const vpColor &color, unsigned int thickness) +void vpKltOpencv::display(const vpImage &I, const std::vector &features, const vpColor &color, + unsigned int thickness) { vpImagePoint ip; for (size_t i = 0; i < features.size(); i++) { @@ -294,10 +279,8 @@ void vpKltOpencv::display(const vpImage &I, \param thickness : Thickness of the points */ -void vpKltOpencv::display(const vpImage &I, - const std::vector &features, - const std::vector &featuresid, - const vpColor &color, unsigned int thickness) +void vpKltOpencv::display(const vpImage &I, const std::vector &features, + const std::vector &featuresid, const vpColor &color, unsigned int thickness) { vpImagePoint ip; for (size_t i = 0; i < features.size(); i++) { @@ -326,10 +309,8 @@ void vpKltOpencv::display(const vpImage &I, \param thickness : Thickness of the points */ -void vpKltOpencv::display(const vpImage &I, - const std::vector &features, - const std::vector &featuresid, - const vpColor &color, unsigned int thickness) +void vpKltOpencv::display(const vpImage &I, const std::vector &features, + const std::vector &featuresid, const vpColor &color, unsigned int thickness) { vpImagePoint ip; for (size_t i = 0; i < features.size(); i++) { @@ -350,10 +331,7 @@ void vpKltOpencv::display(const vpImage &I, \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; -} +void vpKltOpencv::setMaxFeatures(const int maxCount) { m_maxCount = maxCount; } /*! Set the window size used to refine the corner locations. @@ -375,10 +353,7 @@ void vpKltOpencv::setWindowSize(const int winSize) { m_winSize = winSize; } 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; -} +void vpKltOpencv::setQuality(double qualityLevel) { m_qualityLevel = qualityLevel; } /*! Set the free parameter of the Harris detector. @@ -386,10 +361,7 @@ void vpKltOpencv::setQuality(double qualityLevel) \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; -} +void vpKltOpencv::setHarrisFreeParameter(double harris_k) { m_harris_k = harris_k; } /*! Set the parameter indicating whether to use a Harris detector or @@ -397,10 +369,7 @@ void vpKltOpencv::setHarrisFreeParameter(double harris_k) \param useHarrisDetector : If 1 (default value), use the Harris detector. If 0 use the eigenvalue. */ -void vpKltOpencv::setUseHarris(const int useHarrisDetector) -{ - m_useHarrisDetector = useHarrisDetector; -} +void vpKltOpencv::setUseHarris(const int useHarrisDetector) { m_useHarrisDetector = useHarrisDetector; } /*! Set the minimal Euclidean distance between detected corners during @@ -409,20 +378,14 @@ void vpKltOpencv::setUseHarris(const int useHarrisDetector) \param minDistance : Minimal possible Euclidean distance between the detected corners. Default value is set to 15. */ -void vpKltOpencv::setMinDistance(double minDistance) -{ - m_minDistance = minDistance; -} +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; -} +void vpKltOpencv::setMinEigThreshold(double minEigThreshold) { m_minEigThreshold = minEigThreshold; } /*! Set the size of the averaging block used to track the features. @@ -433,10 +396,7 @@ void vpKltOpencv::setMinEigThreshold(double minEigThreshold) \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; -} +void vpKltOpencv::setBlockSize(const int blockSize) { m_blockSize = blockSize; } /*! Set the maximal pyramid level. If the level is zero, then no pyramid is @@ -446,10 +406,7 @@ void vpKltOpencv::setBlockSize(const int blockSize) 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; -} +void vpKltOpencv::setPyramidLevels(const int pyrMaxLevel) { m_pyrMaxLevel = pyrMaxLevel; } /*! Set the points that will be used as initial guess during the next call to @@ -490,8 +447,7 @@ void vpKltOpencv::setInitialGuess(const std::vector &guess_pts) \sa getFeatures(), getFeaturesId \sa initTracking() */ -void vpKltOpencv::setInitialGuess(const std::vector &init_pts, - const std::vector &guess_pts, +void vpKltOpencv::setInitialGuess(const std::vector &init_pts, const std::vector &guess_pts, const std::vector &fid) { if (guess_pts.size() != init_pts.size()) { @@ -515,8 +471,7 @@ void vpKltOpencv::setInitialGuess(const std::vector &init_pts, \param pts : Vector of points that should be tracked. */ -void vpKltOpencv::initTracking(const cv::Mat &I, - const std::vector &pts) +void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector &pts) { m_initial_guess = false; m_points[1] = pts; @@ -529,9 +484,7 @@ void vpKltOpencv::initTracking(const cv::Mat &I, I.copyTo(m_gray); } -void vpKltOpencv::initTracking(const cv::Mat &I, - const std::vector &pts, - const std::vector &ids) +void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector &pts, const std::vector &ids) { m_initial_guess = false; m_points[1] = pts; @@ -609,8 +562,7 @@ void vpKltOpencv::addFeature(const cv::Point2f &f) void vpKltOpencv::suppressFeature(const int &index) { if ((size_t)index >= m_points[1].size()) { - throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", - index)); + throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index)); } m_points[1].erase(m_points[1].begin() + index); @@ -676,19 +628,15 @@ 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) + : 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])); + 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)); @@ -699,14 +647,12 @@ vpKltOpencv::vpKltOpencv() Copy constructor. */ vpKltOpencv::vpKltOpencv(const 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) + : 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; } @@ -770,51 +716,44 @@ vpKltOpencv &vpKltOpencv::operator=(const vpKltOpencv ©) } if (copy.prev_pyramid) { - prev_pyramid = - cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1); + 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)); + /*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)); + /*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)); + /*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)); + /*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)); + /*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)); + /*bool **/ lostDuringTrack = (bool *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(bool)); for (int i = 0; i < copy.maxFeatures; i++) lostDuringTrack[i] = copy.lostDuringTrack[i]; } @@ -849,10 +788,8 @@ void vpKltOpencv::setMaxFeatures(const int input) if (prev_featuresid) cvFree(&prev_featuresid); - features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * - sizeof(CvPoint2D32f)); - prev_features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * - sizeof(CvPoint2D32f)); + 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)); @@ -872,19 +809,16 @@ void vpKltOpencv::setMaxFeatures(const int input) void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask) { if (!I) { - throw(vpException(vpTrackingException::initializationError, - "Image Not initialized")); + throw(vpException(vpTrackingException::initializationError, "Image Not initialized")); } if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) { - throw(vpException(vpTrackingException::initializationError, - "Bad Image format")); + 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")); + throw(vpException(vpTrackingException::initializationError, "Bad Image format")); } } @@ -897,8 +831,7 @@ void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask) 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) { + if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) { reset(); image = cvCreateImage(cvGetSize(I), 8, 1); image->origin = I->origin; @@ -924,13 +857,10 @@ void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask) 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, + 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)); + 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); @@ -943,8 +873,7 @@ void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask) globalcountFeatures++; if (OnNewFeature) { - OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x, - features[boucle].y); + OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x, features[boucle].y); } } } @@ -960,8 +889,7 @@ void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask) void vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, int size) { if (size > maxFeatures) - throw(vpException(vpTrackingException::initializationError, - "Cannot initialize tracker from points")); + throw(vpException(vpTrackingException::initializationError, "Cannot initialize tracker from points")); // Creation des buffers CvSize Sizeim, SizeI; @@ -972,8 +900,7 @@ void vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, int size) 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) { + if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) { reset(); image = cvCreateImage(cvGetSize(I), 8, 1); image->origin = I->origin; @@ -996,12 +923,10 @@ void vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, int size) cvCopy(I, image, 0); } -void vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, - long *fid, int size) +void vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size) { if (size > maxFeatures) - throw(vpException(vpTrackingException::initializationError, - "Cannot initialize tracker from points")); + throw(vpException(vpTrackingException::initializationError, "Cannot initialize tracker from points")); // Creation des buffers CvSize Sizeim, SizeI; @@ -1012,8 +937,7 @@ void vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, 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) { + if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) { reset(); image = cvCreateImage(cvGetSize(I), 8, 1); image->origin = I->origin; @@ -1043,18 +967,15 @@ void vpKltOpencv::track(const IplImage *I) { if (!initialized) { vpERROR_TRACE("KLT Not initialized"); - throw(vpException(vpTrackingException::initializationError, - "KLT Not initialized")); + throw(vpException(vpTrackingException::initializationError, "KLT Not initialized")); } if (!I) { - throw(vpException(vpTrackingException::initializationError, - "Image Not initialized")); + throw(vpException(vpTrackingException::initializationError, "Image Not initialized")); } if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) { - throw(vpException(vpTrackingException::initializationError, - "Bad Image format")); + throw(vpException(vpTrackingException::initializationError, "Bad Image format")); } CV_SWAP(prev_image, image, swap_temp); @@ -1076,10 +997,9 @@ void vpKltOpencv::track(const IplImage *I) 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); + 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; @@ -1124,16 +1044,14 @@ void vpKltOpencv::track(const IplImage *I) \param color : Color used to display the features. \param thickness : Thickness of the drawings. */ -void vpKltOpencv::display(const vpImage &I, vpColor color, - unsigned int thickness) +void vpKltOpencv::display(const vpImage &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); + vpKltOpencv::display(I, features, featuresid, countFeatures, color, thickness); } /*! @@ -1203,9 +1121,7 @@ void vpKltOpencv::setInitialGuess(CvPoint2D32f **guess_pts) \sa getFeatures(), getFeaturesId \sa initTracking() */ -void vpKltOpencv::setInitialGuess(CvPoint2D32f **init_pts, - CvPoint2D32f **guess_pts, long *fid, - int size) +void vpKltOpencv::setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, long *fid, int size) { countPrevFeatures = size; countFeatures = size; @@ -1298,10 +1214,8 @@ void vpKltOpencv::suppressFeature(int index) \param thickness : Thickness of the points. */ -void vpKltOpencv::display(const vpImage &I, - const CvPoint2D32f *features_list, - const int &nbFeatures, vpColor color, - unsigned int thickness) +void vpKltOpencv::display(const vpImage &I, const CvPoint2D32f *features_list, const int &nbFeatures, + vpColor color, unsigned int thickness) { vpImagePoint ip; for (int i = 0; i < nbFeatures; i++) { @@ -1324,10 +1238,8 @@ void vpKltOpencv::display(const vpImage &I, \param thickness : Thickness of the points. */ -void vpKltOpencv::display(const vpImage &I, - const CvPoint2D32f *features_list, - const int &nbFeatures, vpColor color, - unsigned int thickness) +void vpKltOpencv::display(const vpImage &I, const CvPoint2D32f *features_list, const int &nbFeatures, + vpColor color, unsigned int thickness) { vpImagePoint ip; for (int i = 0; i < nbFeatures; i++) { @@ -1353,10 +1265,8 @@ void vpKltOpencv::display(const vpImage &I, \param thickness : Thickness of the points */ -void vpKltOpencv::display(const vpImage &I, - const CvPoint2D32f *features_list, - const long *featuresid_list, const int &nbFeatures, - vpColor color, unsigned int thickness) +void vpKltOpencv::display(const vpImage &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++) { @@ -1387,10 +1297,8 @@ void vpKltOpencv::display(const vpImage &I, \param thickness : Thickness of the points */ -void vpKltOpencv::display(const vpImage &I, - const CvPoint2D32f *features_list, - const long *featuresid_list, const int &nbFeatures, - vpColor color, unsigned int thickness) +void vpKltOpencv::display(const vpImage &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++) { diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthDenseTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthDenseTracker.h index 6d6e2aae97..8e8d2b1f20 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthDenseTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthDenseTracker.h @@ -51,38 +51,25 @@ class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker vpMbDepthDenseTracker(); virtual ~vpMbDepthDenseTracker(); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); - - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); virtual inline vpColVector getError() const { return m_error_depthDense; } - virtual inline vpColVector getRobustWeights() const - { - return m_w_depthDense; - } + virtual inline vpColVector getRobustWeights() const { return m_w_depthDense; } virtual void init(const vpImage &I); virtual void loadConfigFile(const std::string &configFile); - void reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, + void reInitModel(const vpImage &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose = false); #if defined(VISP_HAVE_PCL) - void - reInitModel(const pcl::PointCloud::ConstPtr &point_cloud, - const std::string &cad_name, const vpHomogeneousMatrix &cMo_, - const bool verbose = false); + void reInitModel(const pcl::PointCloud::ConstPtr &point_cloud, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose = false); #endif virtual void resetTracker(); @@ -92,11 +79,9 @@ class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker virtual void setDepthDenseFilteringMaxDistance(const double maxDistance); virtual void setDepthDenseFilteringMethod(const int method); virtual void setDepthDenseFilteringMinDistance(const double minDistance); - virtual void - setDepthDenseFilteringOccupancyRatio(const double occupancyRatio); + virtual void setDepthDenseFilteringOccupancyRatio(const double occupancyRatio); - inline void setDepthDenseSamplingStep(const unsigned int stepX, - const unsigned int stepY) + inline void setDepthDenseSamplingStep(const unsigned int stepX, const unsigned int stepY) { if (stepX == 0 || stepY == 0) { std::cerr << "stepX and stepY must be greater than zero!" << std::endl; @@ -109,12 +94,9 @@ class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker virtual void setOgreVisibilityTest(const bool &v); - virtual void setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo); + virtual void setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo); #ifdef VISP_HAVE_PCL - virtual void - setPose(const pcl::PointCloud::ConstPtr &point_cloud, - const vpHomogeneousMatrix &cdMo); + virtual void setPose(const pcl::PointCloud::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo); #endif virtual void setScanLineVisibilityTest(const bool &v); @@ -123,11 +105,9 @@ class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker virtual void track(const vpImage &); #ifdef VISP_HAVE_PCL - virtual void - track(const pcl::PointCloud::ConstPtr &point_cloud); + virtual void track(const pcl::PointCloud::ConstPtr &point_cloud); #endif - virtual void track(const std::vector &point_cloud, - const unsigned int width, const unsigned int height); + virtual void track(const std::vector &point_cloud, const unsigned int width, const unsigned int height); protected: //! Set of faces describing the object used only for display with scan line. @@ -169,12 +149,10 @@ class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker virtual void computeVVSWeights(); using vpMbTracker::computeVVSWeights; - virtual void initCircle(const vpPoint &p1, const vpPoint &p2, - const vpPoint &p3, const double radius, + 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, + 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); @@ -182,10 +160,9 @@ class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker virtual void initFaceFromLines(vpMbtPolygon &polygon); #ifdef VISP_HAVE_PCL - void segmentPointCloud( - const pcl::PointCloud::ConstPtr &point_cloud); + void segmentPointCloud(const pcl::PointCloud::ConstPtr &point_cloud); #endif - void segmentPointCloud(const std::vector &point_cloud, - const unsigned int width, const unsigned int height); + void segmentPointCloud(const std::vector &point_cloud, const unsigned int width, + const unsigned int height); }; #endif diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h index d2892d657e..14da14db39 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h @@ -51,77 +51,55 @@ class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker vpMbDepthNormalTracker(); virtual ~vpMbDepthNormalTracker(); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); - - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); - - virtual inline vpMbtFaceDepthNormal::vpFeatureEstimationType - getDepthFeatureEstimationMethod() const + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + + virtual inline vpMbtFaceDepthNormal::vpFeatureEstimationType getDepthFeatureEstimationMethod() const { return m_depthNormalFeatureEstimationMethod; } virtual inline vpColVector getError() const { return m_error_depthNormal; } - virtual inline vpColVector getRobustWeights() const - { - return m_w_depthNormal; - } + virtual inline vpColVector getRobustWeights() const { return m_w_depthNormal; } virtual void init(const vpImage &I); virtual void loadConfigFile(const std::string &configFile); - void reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, + void reInitModel(const vpImage &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose = false); #if defined(VISP_HAVE_PCL) - void - reInitModel(const pcl::PointCloud::ConstPtr &point_cloud, - const std::string &cad_name, const vpHomogeneousMatrix &cMo_, - const bool verbose = false); + void reInitModel(const pcl::PointCloud::ConstPtr &point_cloud, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose = false); #endif virtual void resetTracker(); virtual void setCameraParameters(const vpCameraParameters &camera); - virtual void setDepthNormalFaceCentroidMethod( - const vpMbtFaceDepthNormal::vpFaceCentroidType &method); + virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method); - virtual void setDepthNormalFeatureEstimationMethod( - const vpMbtFaceDepthNormal::vpFeatureEstimationType &method); + virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method); virtual void setDepthNormalPclPlaneEstimationMethod(const int method); - virtual void - setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter); + virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter); - virtual void - setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold); + virtual void setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold); - virtual void setDepthNormalSamplingStep(const unsigned int stepX, - const unsigned int stepY); + virtual void setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY); // virtual void setDepthNormalUseRobust(const bool use); virtual void setOgreVisibilityTest(const bool &v); - virtual void setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo); + virtual void setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo); #if defined(VISP_HAVE_PCL) - virtual void - setPose(const pcl::PointCloud::ConstPtr &point_cloud, - const vpHomogeneousMatrix &cdMo); + virtual void setPose(const pcl::PointCloud::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo); #endif virtual void setScanLineVisibilityTest(const bool &v); @@ -130,16 +108,13 @@ class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker virtual void track(const vpImage &); #if defined(VISP_HAVE_PCL) - virtual void - track(const pcl::PointCloud::ConstPtr &point_cloud); + virtual void track(const pcl::PointCloud::ConstPtr &point_cloud); #endif - virtual void track(const std::vector &point_cloud, - const unsigned int width, const unsigned int height); + virtual void track(const std::vector &point_cloud, const unsigned int width, const unsigned int height); protected: //! Method to estimate the desired features - vpMbtFaceDepthNormal::vpFeatureEstimationType - m_depthNormalFeatureEstimationMethod; + vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod; //! Set of faces describing the object used only for display with scan line. vpMbHiddenFaces m_depthNormalHiddenFacesDisplay; //! Dummy image used to compute the visibility @@ -185,12 +160,10 @@ class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker virtual void computeVVSInit(); virtual void computeVVSInteractionMatrixAndResidu(); - virtual void initCircle(const vpPoint &p1, const vpPoint &p2, - const vpPoint &p3, const double radius, + 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, + 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); @@ -198,10 +171,9 @@ class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker virtual void initFaceFromLines(vpMbtPolygon &polygon); #ifdef VISP_HAVE_PCL - void segmentPointCloud( - const pcl::PointCloud::ConstPtr &point_cloud); + void segmentPointCloud(const pcl::PointCloud::ConstPtr &point_cloud); #endif - void segmentPointCloud(const std::vector &point_cloud, - const unsigned int width, const unsigned int height); + void segmentPointCloud(const std::vector &point_cloud, const unsigned int width, + const unsigned int height); }; #endif diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltMultiTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltMultiTracker.h index 4e985d3cdb..689b300443 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltMultiTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltMultiTracker.h @@ -51,8 +51,7 @@ #include -#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && \ - (VISP_HAVE_OPENCV_VERSION >= 0x020100) +#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100) #include #include @@ -73,8 +72,7 @@ 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. */ -class VISP_EXPORT vpMbEdgeKltMultiTracker : public vpMbEdgeMultiTracker, - public vpMbKltMultiTracker +class VISP_EXPORT vpMbEdgeKltMultiTracker : public vpMbEdgeMultiTracker, public vpMbKltMultiTracker { protected: //! Factor for KLT trackers. @@ -88,8 +86,7 @@ class VISP_EXPORT vpMbEdgeKltMultiTracker : public vpMbEdgeMultiTracker, //! Map of camera transformation matrix between the current camera frame to //! the reference camera frame (cCurrent_M_cRef) - std::map - m_mapOfCameraTransformationMatrix; + std::map m_mapOfCameraTransformationMatrix; //! Name of the reference camera std::string m_referenceCameraName; @@ -111,57 +108,37 @@ class VISP_EXPORT vpMbEdgeKltMultiTracker : public vpMbEdgeMultiTracker, virtual ~vpMbEdgeKltMultiTracker(); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); - virtual void - display(const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, const vpCameraParameters &cam2, - const vpColor &color, const unsigned int thickness = 1, - const bool displayFullModel = false); - - virtual void display(const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, - const vpCameraParameters &cam2, const vpColor &color, - const unsigned int thickness = 1, + virtual void display(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1, + const vpCameraParameters &cam2, const vpColor &color, const unsigned int thickness = 1, const bool displayFullModel = false); - virtual void display( - const std::map *> - &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - - virtual void display( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I1, const vpImage &I2, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1, const vpCameraParameters &cam2, + const vpColor &color, const unsigned int thickness = 1, const bool displayFullModel = false); + + virtual void display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, const vpColor &col, + const unsigned int thickness = 1, const bool displayFullModel = false); + + virtual void display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, const vpColor &col, + const unsigned int thickness = 1, const bool displayFullModel = false); virtual std::vector getCameraNames() const; virtual void getCameraParameters(vpCameraParameters &camera) const; - virtual void getCameraParameters(vpCameraParameters &cam1, - vpCameraParameters &cam2) const; - virtual void getCameraParameters(const std::string &cameraName, - vpCameraParameters &camera) const; - virtual void getCameraParameters( - std::map &mapOfCameraParameters) const; + virtual void getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const; + virtual void getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const; + virtual void getCameraParameters(std::map &mapOfCameraParameters) const; using vpMbTracker::getClipping; virtual unsigned int getClipping(const std::string &cameraName) const; @@ -195,115 +172,74 @@ class VISP_EXPORT vpMbEdgeKltMultiTracker : public vpMbEdgeMultiTracker, \return The number of cameras. */ - inline unsigned int getNumberOfCameras() const - { - return (unsigned int)m_mapOfKltTrackers.size(); - } + inline unsigned int getNumberOfCameras() const { return (unsigned int)m_mapOfKltTrackers.size(); } using vpMbKltMultiTracker::getPose; - virtual void getPose(vpHomogeneousMatrix &c1Mo, - vpHomogeneousMatrix &c2Mo) const; - virtual void getPose(const std::string &cameraName, - vpHomogeneousMatrix &cMo_) const; - virtual void - getPose(std::map &mapOfCameraPoses) const; + virtual void getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const; + virtual void getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo_) const; + virtual void getPose(std::map &mapOfCameraPoses) const; virtual inline vpColVector getError() const { return m_error_hybridMulti; } - virtual inline vpColVector getRobustWeights() const - { - return m_w_hybridMulti; - } + virtual inline vpColVector getRobustWeights() const { return m_w_hybridMulti; } virtual void init(const vpImage &I); #ifdef VISP_HAVE_MODULE_GUI - virtual void initClick(const vpImage &I, - const std::vector &points3D_list, + virtual void initClick(const vpImage &I, const std::vector &points3D_list, const std::string &displayFile = ""); - virtual void initClick(const vpImage &I, - const std::string &initFile, - const bool displayHelp = false); + virtual void initClick(const vpImage &I, const std::string &initFile, const bool displayHelp = false); - virtual void initClick(const vpImage &I1, - const vpImage &I2, - const std::string &initFile1, - const std::string &initFile2, - const bool displayHelp = false, + virtual void initClick(const vpImage &I1, const vpImage &I2, + const std::string &initFile1, const std::string &initFile2, const bool displayHelp = false, const bool firstCameraIsReference = true); - virtual void - initClick(const std::map *> - &mapOfImages, - const std::string &initFile, const bool displayHelp = false); + virtual void initClick(const std::map *> &mapOfImages, + const std::string &initFile, const bool displayHelp = false); - virtual void - initClick(const std::map *> - &mapOfImages, - const std::map &mapOfInitFiles, - const bool displayHelp = false); + virtual void initClick(const std::map *> &mapOfImages, + const std::map &mapOfInitFiles, const bool displayHelp = false); #endif - virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, - const double, const int, + 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 = ""); - - virtual void initFromPose(const vpImage &I, - const std::string &initFile); - virtual void initFromPose(const vpImage &I, - const vpHomogeneousMatrix &cMo); - virtual void initFromPose(const vpImage &I, - const vpPoseVector &cPo); - - virtual void initFromPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, + virtual void initCylinder(const vpPoint &, const vpPoint &, const double, const int, const std::string &name = ""); + + virtual void initFromPose(const vpImage &I, const std::string &initFile); + virtual void initFromPose(const vpImage &I, const vpHomogeneousMatrix &cMo); + virtual void initFromPose(const vpImage &I, const vpPoseVector &cPo); + + virtual void initFromPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference = true); - virtual void - initFromPose(const std::map *> - &mapOfImages, - const vpHomogeneousMatrix &cMo_); - virtual void initFromPose( - const std::map *> - &mapOfImages, - const std::map &mapOfCameraPoses); + virtual void initFromPose(const std::map *> &mapOfImages, + const vpHomogeneousMatrix &cMo_); + virtual void initFromPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses); virtual void initFaceFromCorners(vpMbtPolygon &polygon); virtual void initFaceFromLines(vpMbtPolygon &polygon); virtual void loadConfigFile(const std::string &configFile); - virtual void loadConfigFile(const std::string &configFile1, - const std::string &configFile2, + virtual void loadConfigFile(const std::string &configFile1, const std::string &configFile2, const bool firstCameraIsReference = true); - virtual void - loadConfigFile(const std::map &mapOfConfigFiles); + virtual void loadConfigFile(const std::map &mapOfConfigFiles); - virtual void loadModel(const std::string &modelFile, - const bool verbose = false); + virtual void loadModel(const std::string &modelFile, const bool verbose = false); - virtual void reInitModel(const vpImage &I, + virtual void reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose = false); + virtual void reInitModel(const vpImage &I1, const vpImage &I2, + const std::string &cad_name, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const bool verbose = false, + const bool firstCameraIsReference = true); + virtual void reInitModel(const std::map *> &mapOfImages, const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, + const std::map &mapOfCameraPoses, const bool verbose = false); - virtual void reInitModel(const vpImage &I1, - const vpImage &I2, - const std::string &cad_name, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, - const bool verbose = false, - const bool firstCameraIsReference = true); - virtual void reInitModel( - const std::map *> - &mapOfImages, - const std::string &cad_name, - const std::map &mapOfCameraPoses, - const bool verbose = false); virtual void resetTracker(); @@ -312,35 +248,28 @@ class VISP_EXPORT vpMbEdgeKltMultiTracker : public vpMbEdgeMultiTracker, virtual void setCameraParameters(const vpCameraParameters &camera); - virtual void setCameraParameters(const vpCameraParameters &camera1, - const vpCameraParameters &camera2, + virtual void setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2, const bool firstCameraIsReference = true); - virtual void setCameraParameters(const std::string &cameraName, - const vpCameraParameters &camera); + virtual void setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera); - virtual void setCameraParameters( - const std::map &mapOfCameraParameters); + virtual void setCameraParameters(const std::map &mapOfCameraParameters); - virtual void setCameraTransformationMatrix( - const std::string &cameraName, - const vpHomogeneousMatrix &cameraTransformationMatrix); + virtual void setCameraTransformationMatrix(const std::string &cameraName, + const vpHomogeneousMatrix &cameraTransformationMatrix); - virtual void setCameraTransformationMatrix( - const std::map - &mapOfTransformationMatrix); + virtual void + setCameraTransformationMatrix(const std::map &mapOfTransformationMatrix); virtual void setClipping(const unsigned int &flags); - virtual void setClipping(const std::string &cameraName, - const unsigned int &flags); + virtual void setClipping(const std::string &cameraName, const unsigned int &flags); virtual void setCovarianceComputation(const bool &flag); virtual void setDisplayFeatures(const bool displayF); virtual void setFarClippingDistance(const double &dist); - virtual void setFarClippingDistance(const std::string &cameraName, - const double &dist); + virtual void setFarClippingDistance(const std::string &cameraName, const double &dist); /*! Set the factor for KLT tracker in the VVS process. @@ -381,24 +310,18 @@ class VISP_EXPORT vpMbEdgeKltMultiTracker : public vpMbEdgeMultiTracker, #endif virtual void setLod(const bool useLod, const std::string &name = ""); - virtual void setLod(const bool useLod, const std::string &cameraName, - const std::string &name); + virtual void setLod(const bool useLod, const std::string &cameraName, const std::string &name); - virtual void setMinLineLengthThresh(const double minLineLengthThresh, - const std::string &name = ""); - virtual void setMinLineLengthThresh(const double minLineLengthThresh, - const std::string &cameraName, + virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name = ""); + virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &cameraName, const std::string &name); - virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, - const std::string &name = ""); - virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, - const std::string &cameraName, + virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name = ""); + virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName, const std::string &name); virtual void setNearClippingDistance(const double &dist); - virtual void setNearClippingDistance(const std::string &cameraName, - const double &dist); + virtual void setNearClippingDistance(const std::string &cameraName, const double &dist); virtual void setOgreShowConfigDialog(const bool showConfigDialog); @@ -406,24 +329,17 @@ class VISP_EXPORT vpMbEdgeKltMultiTracker : public vpMbEdgeMultiTracker, virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt); - virtual void setPose(const vpImage &I, - const vpHomogeneousMatrix &cMo); + virtual void setPose(const vpImage &I, const vpHomogeneousMatrix &cMo); - virtual void setPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, + virtual void setPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference = true); - virtual void - setPose(const std::map *> - &mapOfImages, - const vpHomogeneousMatrix &cMo_); + virtual void setPose(const std::map *> &mapOfImages, + const vpHomogeneousMatrix &cMo_); - virtual void - setPose(const std::map *> - &mapOfImages, - const std::map &mapOfCameraPoses); + virtual void setPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses); virtual void setProjectionErrorComputation(const bool &flag); @@ -436,10 +352,8 @@ class VISP_EXPORT vpMbEdgeKltMultiTracker : public vpMbEdgeMultiTracker, virtual void testTracking(); virtual void track(const vpImage &I); - virtual void track(const vpImage &I1, - const vpImage &I2); - virtual void - track(std::map *> &mapOfImages); + virtual void track(const vpImage &I1, const vpImage &I2); + virtual void track(std::map *> &mapOfImages); protected: // Current computeVVS function hides the base computeVVS but it is intended. @@ -448,34 +362,28 @@ class VISP_EXPORT vpMbEdgeKltMultiTracker : public vpMbEdgeMultiTracker, using vpMbEdgeMultiTracker::computeVVS; using vpMbKltMultiTracker::computeVVS; - virtual void computeVVS( - std::map *> &mapOfImages, - const unsigned int lvl = 0); + virtual void computeVVS(std::map *> &mapOfImages, + const unsigned int lvl = 0); virtual void computeVVSInit(); virtual void computeVVSInteractionMatrixAndResidu(); using vpMbEdgeMultiTracker::computeVVSInteractionMatrixAndResidu; using vpMbKltMultiTracker::computeVVSInteractionMatrixAndResidu; - virtual void computeVVSInteractionMatrixAndResidu( - std::map *> &mapOfImages, - std::map &mapOfVelocityTwist); + virtual void computeVVSInteractionMatrixAndResidu(std::map *> &mapOfImages, + std::map &mapOfVelocityTwist); virtual void computeVVSWeights(); using vpMbTracker::computeVVSWeights; - virtual unsigned int initMbtTracking( - std::map *> &mapOfImages, - unsigned int lvl); + virtual unsigned int initMbtTracking(std::map *> &mapOfImages, + unsigned int lvl); // Same thing as computeVVS using vpMbKltMultiTracker::postTracking; - virtual void postTracking( - std::map *> &mapOfImages, - const unsigned int lvl); + virtual void postTracking(std::map *> &mapOfImages, const unsigned int lvl); virtual void reinit(/*const vpImage& I*/); - virtual void trackMovingEdges( - std::map *> &mapOfImages); + virtual void trackMovingEdges(std::map *> &mapOfImages); }; #endif // VISP_HAVE_OPENCV diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltTracker.h index d722e09f48..a991bf2938 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltTracker.h @@ -47,8 +47,7 @@ #include -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) #include #include @@ -106,12 +105,13 @@ int main() #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. + // Load the 3d model in cao format. No 3rd party library is required + tracker.loadModel("cube.cao"); + // Get the camera parameters used by the tracker (from the configuration file). + tracker.getCameraParameters(cam); + // Initialise manually the pose by clicking on the image points associated to the 3d points contained in the + // cube.init file. + tracker.initClick(I, "cube.init"); while(true){ // Acquire a new image @@ -119,13 +119,14 @@ cube.init file. 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); + 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 + // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeKltTracker::loadConfigFile() + vpXmlParser::cleanup(); +#endif return 0; #endif @@ -148,8 +149,7 @@ int main() #if defined VISP_HAVE_OPENCV vpMbEdgeKltTracker tracker; // Create an hybrid model based tracker. vpImage I; - vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then -computed using the tracker. + 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 @@ -157,8 +157,8 @@ computed using the tracker. #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. + // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used. + tracker.loadModel("cube.cao"); tracker.initFromPose(I, cMo); // initialise the tracker with the given pose. while(true){ @@ -168,8 +168,9 @@ is required, if coin is not installed .cao file can be used. } #if defined VISP_HAVE_XML2 - // Cleanup memory allocated by xml library used to parse the xml config file -in vpMbEdgeKltTracker::loadConfigFile() vpXmlParser::cleanup(); #endif + // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeKltTracker::loadConfigFile() + vpXmlParser::cleanup(); +#endif return 0; #endif @@ -206,30 +207,29 @@ int main() #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. + tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file). + // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used. + tracker.loadModel("cube.cao"); 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); + 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 + // 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 +class VISP_EXPORT vpMbEdgeKltTracker : public vpMbKltTracker, public vpMbEdgeTracker { protected: //! The threshold used in the robust estimation of KLT. @@ -251,16 +251,10 @@ class VISP_EXPORT vpMbEdgeKltTracker : public vpMbKltTracker, vpMbEdgeKltTracker(); virtual ~vpMbEdgeKltTracker(); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); virtual inline vpColVector getError() const { return m_error_hybrid; } @@ -271,20 +265,14 @@ class VISP_EXPORT vpMbEdgeKltTracker : public vpMbKltTracker, \return Near clipping value. */ - virtual inline double getNearClippingDistance() const - { - return vpMbKltTracker::getNearClippingDistance(); - } + virtual inline double getNearClippingDistance() const { return vpMbKltTracker::getNearClippingDistance(); } void loadConfigFile(const char *configFile); virtual void loadConfigFile(const std::string &configFile); - void reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, + void reInitModel(const vpImage &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose = false); - void reInitModel(const vpImage &I, const char *cad_name, - const vpHomogeneousMatrix &cMo, + void reInitModel(const vpImage &I, const char *cad_name, const vpHomogeneousMatrix &cMo, const bool verbose = false); void resetTracker(); @@ -297,30 +285,21 @@ class VISP_EXPORT vpMbEdgeKltTracker : public vpMbKltTracker, \param flags : New clipping flags. */ - virtual void setClipping(const unsigned int &flags) - { - vpMbEdgeTracker::setClipping(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); - } + virtual void setFarClippingDistance(const double &dist) { vpMbEdgeTracker::setFarClippingDistance(dist); } /*! Set the near distance for clipping. \param dist : Near clipping value. */ - virtual void setNearClippingDistance(const double &dist) - { - vpMbEdgeTracker::setNearClippingDistance(dist); - } + virtual void setNearClippingDistance(const double &dist) { vpMbEdgeTracker::setNearClippingDistance(dist); } /*! Use Ogre3D for visibility tests @@ -349,25 +328,20 @@ class VISP_EXPORT vpMbEdgeKltTracker : public vpMbKltTracker, vpMbKltTracker::setScanLineVisibilityTest(v); } - virtual void setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo); + virtual void setPose(const vpImage &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 setProjectionErrorComputation(const bool &flag) { vpMbEdgeTracker::setProjectionErrorComputation(flag); } virtual void testTracking() {} virtual void track(const vpImage &I); protected: - virtual void computeVVS(const vpImage &I, - const unsigned int &nbInfos, unsigned int &nbrow, + virtual void computeVVS(const vpImage &I, const unsigned int &nbInfos, unsigned int &nbrow, const unsigned int lvl = 0); virtual void computeVVSInit(); virtual void computeVVSInteractionMatrixAndResidu(); @@ -375,24 +349,20 @@ class VISP_EXPORT vpMbEdgeKltTracker : public vpMbKltTracker, using vpMbTracker::computeVVSPoseEstimation; virtual void init(const vpImage &I); - virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, - const double r, const int idFace = 0, + 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 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 &I, vpColVector &w_mbt, - vpColVector &w_klt, const unsigned int lvl = 0); + bool postTracking(const vpImage &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 &I, - vpColVector &factor, - const unsigned int lvl = 0); - void trackSecondLoop(const vpImage &I, vpMatrix &L, - vpColVector &_error, vpHomogeneousMatrix &cMo, + unsigned int trackFirstLoop(const vpImage &I, vpColVector &factor, const unsigned int lvl = 0); + void trackSecondLoop(const vpImage &I, vpMatrix &L, vpColVector &_error, vpHomogeneousMatrix &cMo, const unsigned int lvl = 0); }; diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeMultiTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeMultiTracker.h index 84b692bc65..043373864e 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeMultiTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeMultiTracker.h @@ -71,15 +71,13 @@ class VISP_EXPORT vpMbEdgeMultiTracker : public vpMbEdgeTracker protected: //! Map of camera transformation matrix between the current camera frame to //! the reference camera frame (cCurrent_M_cRef) - std::map - m_mapOfCameraTransformationMatrix; + std::map m_mapOfCameraTransformationMatrix; //! Map of Model-based edge trackers std::map m_mapOfEdgeTrackers; //! Map of pyramidal images for each camera - std::map *> > - m_mapOfPyramidalImages; + std::map *> > m_mapOfPyramidalImages; //! Name of the reference camera std::string m_referenceCameraName; @@ -105,83 +103,55 @@ class VISP_EXPORT vpMbEdgeMultiTracker : public vpMbEdgeTracker /** @name Inherited functionalities from vpMbEdgeMultiTracker */ //@{ - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); - virtual void - display(const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, const vpCameraParameters &cam2, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - - virtual void display(const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, - const vpCameraParameters &cam2, const vpColor &col, - const unsigned int thickness = 1, + virtual void display(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1, + const vpCameraParameters &cam2, const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); - virtual void display( - const std::map *> - &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - - virtual void display( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I1, const vpImage &I2, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1, const vpCameraParameters &cam2, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + + virtual void display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, const vpColor &col, + const unsigned int thickness = 1, const bool displayFullModel = false); + + virtual void display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, const vpColor &col, + const unsigned int thickness = 1, const bool displayFullModel = false); virtual std::vector getCameraNames() const; virtual void getCameraParameters(vpCameraParameters &camera) const; - virtual void getCameraParameters(vpCameraParameters &cam1, - vpCameraParameters &cam2) const; - virtual void getCameraParameters(const std::string &cameraName, - vpCameraParameters &camera) const; - virtual void getCameraParameters( - std::map &mapOfCameraParameters) const; + virtual void getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const; + virtual void getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const; + virtual void getCameraParameters(std::map &mapOfCameraParameters) const; using vpMbTracker::getClipping; virtual unsigned int getClipping(const std::string &cameraName) const; virtual vpMbHiddenFaces &getFaces(); - virtual vpMbHiddenFaces & - getFaces(const std::string &cameraName); - virtual std::map > - getFaces() const; - - void getLcircle(std::list &circlesList, - const unsigned int level = 0) const; - virtual void getLcircle(const std::string &cameraName, - std::list &circlesList, + virtual vpMbHiddenFaces &getFaces(const std::string &cameraName); + virtual std::map > getFaces() const; + + void getLcircle(std::list &circlesList, const unsigned int level = 0) const; + virtual void getLcircle(const std::string &cameraName, std::list &circlesList, const unsigned int level = 0) const; - void getLcylinder(std::list &cylindersList, - const unsigned int level = 0) const; - virtual void getLcylinder(const std::string &cameraName, - std::list &cylindersList, + void getLcylinder(std::list &cylindersList, const unsigned int level = 0) const; + virtual void getLcylinder(const std::string &cameraName, std::list &cylindersList, const unsigned int level = 0) const; - void getLline(std::list &linesList, - const unsigned int level = 0) const; - virtual void getLline(const std::string &cameraName, - std::list &linesList, + void getLline(std::list &linesList, const unsigned int level = 0) const; + virtual void getLline(const std::string &cameraName, std::list &linesList, const unsigned int level = 0) const; virtual void getMovingEdge(vpMe &p_me) const; @@ -190,8 +160,7 @@ class VISP_EXPORT vpMbEdgeMultiTracker : public vpMbEdgeTracker virtual vpMe getMovingEdge(const std::string &cameraName) const; virtual unsigned int getNbPoints(const unsigned int level = 0) const; - virtual unsigned int getNbPoints(const std::string &cameraName, - const unsigned int level = 0) const; + virtual unsigned int getNbPoints(const std::string &cameraName, const unsigned int level = 0) const; virtual unsigned int getNbPolygon() const; virtual unsigned int getNbPolygon(const std::string &cameraName) const; @@ -202,106 +171,67 @@ class VISP_EXPORT vpMbEdgeMultiTracker : public vpMbEdgeTracker \return The number of cameras. */ - inline unsigned int getNumberOfCameras() const - { - return (unsigned int)m_mapOfEdgeTrackers.size(); - } + inline unsigned int getNumberOfCameras() const { return (unsigned int)m_mapOfEdgeTrackers.size(); } using vpMbTracker::getPose; - virtual void getPose(vpHomogeneousMatrix &c1Mo, - vpHomogeneousMatrix &c2Mo) const; - virtual void getPose(const std::string &cameraName, - vpHomogeneousMatrix &cMo_) const; - virtual void - getPose(std::map &mapOfCameraPoses) const; + virtual void getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const; + virtual void getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo_) const; + virtual void getPose(std::map &mapOfCameraPoses) const; virtual inline vpColVector getError() const { return m_error_edgeMulti; } - virtual inline vpColVector getRobustWeights() const - { - return m_w_edgeMulti; - } + virtual inline vpColVector getRobustWeights() const { return m_w_edgeMulti; } void init(const vpImage &I); #ifdef VISP_HAVE_MODULE_GUI - virtual void initClick(const vpImage &I, - const std::vector &points3D_list, + virtual void initClick(const vpImage &I, const std::vector &points3D_list, const std::string &displayFile = ""); - virtual void initClick(const vpImage &I, - const std::string &initFile, - const bool displayHelp = false); + virtual void initClick(const vpImage &I, const std::string &initFile, const bool displayHelp = false); - virtual void initClick(const vpImage &I1, - const vpImage &I2, - const std::string &initFile1, - const std::string &initFile2, - const bool displayHelp = false, + virtual void initClick(const vpImage &I1, const vpImage &I2, + const std::string &initFile1, const std::string &initFile2, const bool displayHelp = false, const bool firstCameraIsReference = true); - virtual void - initClick(const std::map *> - &mapOfImages, - const std::string &initFile, const bool displayHelp = false); + virtual void initClick(const std::map *> &mapOfImages, + const std::string &initFile, const bool displayHelp = false); - virtual void - initClick(const std::map *> - &mapOfImages, - const std::map &mapOfInitFiles, - const bool displayHelp = false); + virtual void initClick(const std::map *> &mapOfImages, + const std::map &mapOfInitFiles, const bool displayHelp = false); #endif - virtual void initFromPose(const vpImage &I, - const std::string &initFile); - virtual void initFromPose(const vpImage &I, - const vpHomogeneousMatrix &cMo); - virtual void initFromPose(const vpImage &I, - const vpPoseVector &cPo); - - virtual void initFromPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, + virtual void initFromPose(const vpImage &I, const std::string &initFile); + virtual void initFromPose(const vpImage &I, const vpHomogeneousMatrix &cMo); + virtual void initFromPose(const vpImage &I, const vpPoseVector &cPo); + + virtual void initFromPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference = true); - virtual void - initFromPose(const std::map *> - &mapOfImages, - const vpHomogeneousMatrix &cMo_); - virtual void initFromPose( - const std::map *> - &mapOfImages, - const std::map &mapOfCameraPoses); + virtual void initFromPose(const std::map *> &mapOfImages, + const vpHomogeneousMatrix &cMo_); + virtual void initFromPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses); virtual void loadConfigFile(const std::string &configFile); - virtual void loadConfigFile(const std::string &configFile1, - const std::string &configFile2, + virtual void loadConfigFile(const std::string &configFile1, const std::string &configFile2, const bool firstCameraIsReference = true); - virtual void - loadConfigFile(const std::map &mapOfConfigFiles); + virtual void loadConfigFile(const std::map &mapOfConfigFiles); - virtual void loadModel(const std::string &modelFile, - const bool verbose = false); + virtual void loadModel(const std::string &modelFile, const bool verbose = false); - virtual void reInitModel(const vpImage &I, + virtual void reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose = false); + virtual void reInitModel(const vpImage &I1, const vpImage &I2, + const std::string &cad_name, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const bool verbose = false, + const bool firstCameraIsReference = true); + virtual void reInitModel(const std::map *> &mapOfImages, const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, + const std::map &mapOfCameraPoses, const bool verbose = false); - virtual void reInitModel(const vpImage &I1, - const vpImage &I2, - const std::string &cad_name, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, - const bool verbose = false, - const bool firstCameraIsReference = true); - virtual void reInitModel( - const std::map *> - &mapOfImages, - const std::string &cad_name, - const std::map &mapOfCameraPoses, - const bool verbose = false); virtual void resetTracker(); @@ -310,35 +240,28 @@ class VISP_EXPORT vpMbEdgeMultiTracker : public vpMbEdgeTracker virtual void setCameraParameters(const vpCameraParameters &camera); - virtual void setCameraParameters(const vpCameraParameters &camera1, - const vpCameraParameters &camera2, + virtual void setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2, const bool firstCameraIsReference = true); - virtual void setCameraParameters(const std::string &cameraName, - const vpCameraParameters &camera); + virtual void setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera); - virtual void setCameraParameters( - const std::map &mapOfCameraParameters); + virtual void setCameraParameters(const std::map &mapOfCameraParameters); - virtual void setCameraTransformationMatrix( - const std::string &cameraName, - const vpHomogeneousMatrix &cameraTransformationMatrix); + virtual void setCameraTransformationMatrix(const std::string &cameraName, + const vpHomogeneousMatrix &cameraTransformationMatrix); - virtual void setCameraTransformationMatrix( - const std::map - &mapOfTransformationMatrix); + virtual void + setCameraTransformationMatrix(const std::map &mapOfTransformationMatrix); virtual void setClipping(const unsigned int &flags); - virtual void setClipping(const std::string &cameraName, - const unsigned int &flags); + virtual void setClipping(const std::string &cameraName, const unsigned int &flags); virtual void setCovarianceComputation(const bool &flag); virtual void setDisplayFeatures(const bool displayF); virtual void setFarClippingDistance(const double &dist); - virtual void setFarClippingDistance(const std::string &cameraName, - const double &dist); + virtual void setFarClippingDistance(const std::string &cameraName, const double &dist); virtual void setGoodMovingEdgesRatioThreshold(const double threshold); @@ -367,27 +290,21 @@ class VISP_EXPORT vpMbEdgeMultiTracker : public vpMbEdgeTracker #endif virtual void setLod(const bool useLod, const std::string &name = ""); - virtual void setLod(const bool useLod, const std::string &cameraName, - const std::string &name); + virtual void setLod(const bool useLod, const std::string &cameraName, const std::string &name); - virtual void setMinLineLengthThresh(const double minLineLengthThresh, - const std::string &name = ""); - virtual void setMinLineLengthThresh(const double minLineLengthThresh, - const std::string &cameraName, + virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name = ""); + virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &cameraName, const std::string &name); - virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, - const std::string &name = ""); - virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, - const std::string &cameraName, + virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name = ""); + virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName, const std::string &name); virtual void setMovingEdge(const vpMe &me); virtual void setMovingEdge(const std::string &cameraName, const vpMe &me); virtual void setNearClippingDistance(const double &dist); - virtual void setNearClippingDistance(const std::string &cameraName, - const double &dist); + virtual void setNearClippingDistance(const std::string &cameraName, const double &dist); /*! Enable/Disable the appearance of Ogre config dialog on startup. @@ -405,24 +322,17 @@ class VISP_EXPORT vpMbEdgeMultiTracker : public vpMbEdgeTracker virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt); - virtual void setPose(const vpImage &I, - const vpHomogeneousMatrix &cMo); + virtual void setPose(const vpImage &I, const vpHomogeneousMatrix &cMo); - virtual void setPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, + virtual void setPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference = true); - virtual void - setPose(const std::map *> - &mapOfImages, - const vpHomogeneousMatrix &cMo_); + virtual void setPose(const std::map *> &mapOfImages, + const vpHomogeneousMatrix &cMo_); - virtual void - setPose(const std::map *> - &mapOfImages, - const std::map &mapOfCameraPoses); + virtual void setPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses); virtual void setProjectionErrorComputation(const bool &flag); @@ -432,14 +342,11 @@ class VISP_EXPORT vpMbEdgeMultiTracker : public vpMbEdgeTracker virtual void setScanLineVisibilityTest(const bool &v); - virtual void setUseEdgeTracking(const std::string &name, - const bool &useEdgeTracking); + virtual void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking); virtual void track(const vpImage &I); - virtual void track(const vpImage &I1, - const vpImage &I2); - virtual void - track(std::map *> &mapOfImages); + virtual void track(const vpImage &I1, const vpImage &I2); + virtual void track(std::map *> &mapOfImages); //@} protected: @@ -447,31 +354,22 @@ class VISP_EXPORT vpMbEdgeMultiTracker : public vpMbEdgeTracker /** @name Protected Member Functions Inherited from vpMbEdgeMultiTracker */ //@{ - virtual void cleanPyramid( - std::map *> > - &pyramid); + virtual void cleanPyramid(std::map *> > &pyramid); virtual void computeProjectionError(); - virtual void computeVVS( - std::map *> &mapOfImages, - const unsigned int lvl); - virtual void computeVVSFirstPhasePoseEstimation(const unsigned int iter, - bool &isoJoIdentity_); + virtual void computeVVS(std::map *> &mapOfImages, const unsigned int lvl); + virtual void computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_); virtual void computeVVSInit(); virtual void computeVVSInteractionMatrixAndResidu(); using vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu; - virtual void computeVVSInteractionMatrixAndResidu( - std::map *> &mapOfImages, - std::map &mapOfVelocityTwist); + virtual void computeVVSInteractionMatrixAndResidu(std::map *> &mapOfImages, + std::map &mapOfVelocityTwist); virtual void computeVVSWeights(); using vpMbTracker::computeVVSWeights; - virtual void initPyramid( - const std::map *> - &mapOfImages, - std::map *> > - &pyramid); + virtual void initPyramid(const std::map *> &mapOfImages, + std::map *> > &pyramid); //@} }; diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeTracker.h index 9a3327a131..de1a13ad9b 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeTracker.h @@ -130,12 +130,11 @@ int main() #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. + 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 + // Initialise manually the pose by clicking on the image points associated to the 3d points contained in the + // cube.init file. + tracker.initClick(I, "cube.init"); while(true){ // Acquire a new image @@ -143,12 +142,12 @@ cube.init file. 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); + 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(); + // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile() + vpXmlParser::cleanup(); return 0; } @@ -170,11 +169,11 @@ not) them using a vector of booleans, as presented in the following code: 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). + 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 + // 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 @@ -191,8 +190,7 @@ int main() { vpMbEdgeTracker tracker; // Create a model based tracker. vpImage I; - vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then -computed using the tracker. + 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 @@ -200,8 +198,8 @@ computed using the tracker. #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. + // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used. + tracker.loadModel("cube.cao"); tracker.initFromPose(I, cMo); // initialize the tracker with the given pose. while(true){ @@ -210,8 +208,8 @@ is required, if coin is not installed .cao file can be used. tracker.getPose(cMo); // get the pose } - // Cleanup memory allocated by xml library used to parse the xml config file -in vpMbEdgeTracker::loadConfigFile() vpXmlParser::cleanup(); + // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile() + vpXmlParser::cleanup(); return 0; } @@ -246,21 +244,21 @@ int main() #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. + tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file). + // load the 3d model, to read .wrl model coin is required, if coin is not installed + // .cao file can be used. + tracker.loadModel("cube.cao"); 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); + 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(); + // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile() + vpXmlParser::cleanup(); return 0; } @@ -350,21 +348,14 @@ class VISP_EXPORT vpMbEdgeTracker : public virtual vpMbTracker /** @name Inherited functionalities from vpMbEdgeTracker */ //@{ - void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - void display(const vpImage &I, const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); - - void getLline(std::list &linesList, - const unsigned int level = 0) const; - void getLcircle(std::list &circlesList, - const unsigned int level = 0) const; - void getLcylinder(std::list &cylindersList, - const unsigned int level = 0) const; + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + + void getLline(std::list &linesList, const unsigned int level = 0) const; + void getLcircle(std::list &circlesList, const unsigned int level = 0) const; + void getLcylinder(std::list &cylindersList, const unsigned int level = 0) const; /*! Get the moving edge parameters. @@ -397,10 +388,7 @@ class VISP_EXPORT vpMbEdgeTracker : public virtual vpMbTracker \sa setGoodMovingEdgesRatioThreshold() */ - inline double getGoodMovingEdgesRatioThreshold() const - { - return percentageGdPt; - } + inline double getGoodMovingEdgesRatioThreshold() const { return percentageGdPt; } virtual inline vpColVector getError() const { return m_error_edge; } @@ -408,12 +396,9 @@ class VISP_EXPORT vpMbEdgeTracker : public virtual vpMbTracker void loadConfigFile(const std::string &configFile); void loadConfigFile(const char *configFile); - virtual void reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose = false); - void reInitModel(const vpImage &I, const char *cad_name, - const vpHomogeneousMatrix &cMo, + virtual void reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose = false); + void reInitModel(const vpImage &I, const char *cad_name, const vpHomogeneousMatrix &cMo, const bool verbose = false); void resetTracker(); @@ -428,21 +413,16 @@ class VISP_EXPORT vpMbEdgeTracker : public virtual vpMbTracker for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) { (*it)->setCameraParameters(cam); } - for (std::list::const_iterator it = - cylinders[i].begin(); - it != cylinders[i].end(); ++it) { + for (std::list::const_iterator it = cylinders[i].begin(); it != cylinders[i].end(); + ++it) { (*it)->setCameraParameters(cam); } - for (std::list::const_iterator it = - circles[i].begin(); - it != circles[i].end(); ++it) { + for (std::list::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) { (*it)->setCameraParameters(cam); } } @@ -482,9 +462,7 @@ class VISP_EXPORT vpMbEdgeTracker : public virtual vpMbTracker for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) { (*it)->useScanLine = v; } } @@ -504,20 +482,15 @@ class VISP_EXPORT vpMbEdgeTracker : public virtual vpMbTracker \sa getGoodMovingEdgesRatioThreshold() */ - void setGoodMovingEdgesRatioThreshold(const double threshold) - { - percentageGdPt = threshold; - } + void setGoodMovingEdgesRatioThreshold(const double threshold) { percentageGdPt = threshold; } void setMovingEdge(const vpMe &me); - virtual void setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo); + virtual void setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo); void setScales(const std::vector &_scales); - void setUseEdgeTracking(const std::string &name, - const bool &useEdgeTracking); + void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking); void track(const vpImage &I); //@} @@ -526,55 +499,41 @@ class VISP_EXPORT vpMbEdgeTracker : public virtual vpMbTracker /** @name Protected Member Functions Inherited from vpMbEdgeTracker */ //@{ bool samePoint(const vpPoint &P1, const vpPoint &P2) const; - void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, - const double r, int idFace = -1, + 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 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 *> &_pyramid); void computeProjectionError(const vpImage &_I); void computeVVS(const vpImage &_I, const unsigned int lvl); - void computeVVSFirstPhase(const vpImage &I, - const unsigned int iter, double &count, + void computeVVSFirstPhase(const vpImage &I, const unsigned int iter, double &count, const unsigned int lvl = 0); - void computeVVSFirstPhaseFactor(const vpImage &I, - const unsigned int lvl = 0); - void computeVVSFirstPhasePoseEstimation(const unsigned int iter, - bool &isoJoIdentity_); + void computeVVSFirstPhaseFactor(const vpImage &I, const unsigned int lvl = 0); + void computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_); virtual void computeVVSInit(); virtual void computeVVSInteractionMatrixAndResidu(); - virtual void - computeVVSInteractionMatrixAndResidu(const vpImage &I); + virtual void computeVVSInteractionMatrixAndResidu(const vpImage &I); virtual void computeVVSWeights(); using vpMbTracker::computeVVSWeights; - void displayFeaturesOnImage(const vpImage &I, - const unsigned int lvl); + void displayFeaturesOnImage(const vpImage &I, const unsigned int lvl); void downScale(const unsigned int _scale); void init(const vpImage &I); - virtual void initCircle(const vpPoint &p1, const vpPoint &p2, - const vpPoint &p3, const double radius, + 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, + 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); - unsigned int initMbtTracking(unsigned int &nberrors_lines, - unsigned int &nberrors_cylinders, + unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles); - void initMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &_cMo); - void initPyramid(const vpImage &_I, - std::vector *> &_pyramid); + void initMovingEdge(const vpImage &I, const vpHomogeneousMatrix &_cMo); + void initPyramid(const vpImage &_I, std::vector *> &_pyramid); void reInitLevel(const unsigned int _lvl); - void reinitMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &_cMo); + void reinitMovingEdge(const vpImage &I, const vpHomogeneousMatrix &_cMo); void removeCircle(const std::string &name); void removeCylinder(const std::string &name); void removeLine(const std::string &name); @@ -584,8 +543,7 @@ class VISP_EXPORT vpMbEdgeTracker : public virtual vpMbTracker void updateMovingEdge(const vpImage &I); void updateMovingEdgeWeights(); void upScale(const unsigned int _scale); - void visibleFace(const vpImage &_I, - const vpHomogeneousMatrix &_cMo, bool &newvisibleline); + void visibleFace(const vpImage &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline); //@} }; diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h index 9a357d5506..d7acb28180 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h @@ -49,90 +49,61 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker { public: enum vpTrackerType { - EDGE_TRACKER = - 1 << 0, /*!< Model-based tracking using moving edges features. */ -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) + EDGE_TRACKER = 1 << 0, /*!< Model-based tracking using moving edges features. */ +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) KLT_TRACKER = 1 << 1, /*!< Model-based tracking using KLT features. */ #endif - DEPTH_NORMAL_TRACKER = - 1 << 2, /*!< Model-based tracking using depth normal features. */ - DEPTH_DENSE_TRACKER = - 1 << 3 /*!< Model-based tracking using depth dense features. */ + DEPTH_NORMAL_TRACKER = 1 << 2, /*!< Model-based tracking using depth normal features. */ + DEPTH_DENSE_TRACKER = 1 << 3 /*!< Model-based tracking using depth dense features. */ }; vpMbGenericTracker(); - vpMbGenericTracker(const unsigned int nbCameras, - const int trackerType = EDGE_TRACKER); + vpMbGenericTracker(const unsigned int nbCameras, const int trackerType = EDGE_TRACKER); explicit vpMbGenericTracker(const std::vector &trackerTypes); - vpMbGenericTracker(const std::vector &cameraNames, - const std::vector &trackerTypes); + vpMbGenericTracker(const std::vector &cameraNames, const std::vector &trackerTypes); virtual ~vpMbGenericTracker(); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); - virtual void - display(const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, const vpCameraParameters &cam2, - const vpColor &color, const unsigned int thickness = 1, - const bool displayFullModel = false); - virtual void display(const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, - const vpCameraParameters &cam2, const vpColor &color, - const unsigned int thickness = 1, + virtual void display(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1, + const vpCameraParameters &cam2, const vpColor &color, const unsigned int thickness = 1, const bool displayFullModel = false); - - virtual void display( - const std::map *> - &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - virtual void display( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I1, const vpImage &I2, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1, const vpCameraParameters &cam2, + const vpColor &color, const unsigned int thickness = 1, const bool displayFullModel = false); + + virtual void display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, const vpColor &col, + const unsigned int thickness = 1, const bool displayFullModel = false); + virtual void display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, const vpColor &col, + const unsigned int thickness = 1, const bool displayFullModel = false); virtual std::vector getCameraNames() const; using vpMbTracker::getCameraParameters; - virtual void getCameraParameters(vpCameraParameters &cam1, - vpCameraParameters &cam2) const; - virtual void getCameraParameters( - std::map &mapOfCameraParameters) const; + virtual void getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const; + virtual void getCameraParameters(std::map &mapOfCameraParameters) const; virtual std::map getCameraTrackerTypes() const; using vpMbTracker::getClipping; - virtual void getClipping(unsigned int &clippingFlag1, - unsigned int &clippingFlag2) const; - virtual void - getClipping(std::map &mapOfClippingFlags) const; + virtual void getClipping(unsigned int &clippingFlag1, unsigned int &clippingFlag2) const; + virtual void getClipping(std::map &mapOfClippingFlags) const; virtual inline vpColVector getError() const { return m_error; } virtual vpMbHiddenFaces &getFaces(); - virtual vpMbHiddenFaces & - getFaces(const std::string &cameraName); + virtual vpMbHiddenFaces &getFaces(const std::string &cameraName); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) virtual std::list &getFeaturesCircle(); virtual std::list &getFeaturesKltCylinder(); virtual std::list &getFeaturesKlt(); @@ -140,8 +111,7 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual double getGoodMovingEdgesRatioThreshold() const; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) virtual std::vector getKltImagePoints() const; virtual std::map getKltImagePointsWithId() const; @@ -150,8 +120,7 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual vpKltOpencv getKltOpencv() const; virtual void getKltOpencv(vpKltOpencv &klt1, vpKltOpencv &klt2) const; - virtual void - getKltOpencv(std::map &mapOfKlts) const; + virtual void getKltOpencv(std::map &mapOfKlts) const; #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) virtual std::vector getKltPoints() const; @@ -160,49 +129,36 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual double getKltThresholdAcceptation() const; #endif - virtual void getLcircle(const std::string &cameraName, - std::list &circlesList, + virtual void getLcircle(const std::string &cameraName, std::list &circlesList, const unsigned int level = 0) const; - virtual void getLcylinder(const std::string &cameraName, - std::list &cylindersList, + virtual void getLcylinder(const std::string &cameraName, std::list &cylindersList, const unsigned int level = 0) const; - virtual void getLline(const std::string &cameraName, - std::list &linesList, + virtual void getLline(const std::string &cameraName, std::list &linesList, const unsigned int level = 0) const; virtual vpMe getMovingEdge() const; virtual void getMovingEdge(vpMe &me1, vpMe &me2) const; - virtual void - getMovingEdge(std::map &mapOfMovingEdges) const; + virtual void getMovingEdge(std::map &mapOfMovingEdges) const; virtual unsigned int getNbPoints(const unsigned int level = 0) const; - virtual void getNbPoints(std::map &mapOfNbPoints, - const unsigned int level = 0) const; + virtual void getNbPoints(std::map &mapOfNbPoints, const unsigned int level = 0) const; virtual inline unsigned int getNbPolygon() const; - virtual void - getNbPolygon(std::map &mapOfNbPolygons) const; + virtual void getNbPolygon(std::map &mapOfNbPolygons) const; virtual vpMbtPolygon *getPolygon(const unsigned int index); - virtual vpMbtPolygon *getPolygon(const std::string &cameraName, - const unsigned int index); - - virtual std::pair, - std::vector > > - getPolygonFaces(const bool orderPolygons = true, - const bool useVisibility = true, - const bool clipPolygon = false); - virtual void getPolygonFaces( - std::map > &mapOfPolygons, - std::map > > &mapOfPoints, - const bool orderPolygons = true, const bool useVisibility = true, - const bool clipPolygon = false); + virtual vpMbtPolygon *getPolygon(const std::string &cameraName, const unsigned int index); + + virtual std::pair, std::vector > > + getPolygonFaces(const bool orderPolygons = true, const bool useVisibility = true, const bool clipPolygon = false); + virtual void getPolygonFaces(std::map > &mapOfPolygons, + std::map > > &mapOfPoints, + const bool orderPolygons = true, const bool useVisibility = true, + const bool clipPolygon = false); using vpMbTracker::getPose; - virtual void getPose(vpHomogeneousMatrix &c1Mo, - vpHomogeneousMatrix &c2Mo) const; - virtual void - getPose(std::map &mapOfCameraPoses) const; + virtual void getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const; + virtual void getPose(std::map &mapOfCameraPoses) const; virtual inline vpColVector getRobustWeights() const { return m_w; } @@ -210,161 +166,107 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker #ifdef VISP_HAVE_MODULE_GUI using vpMbTracker::initClick; - virtual void initClick(const vpImage &I1, - const vpImage &I2, - const std::string &initFile1, - const std::string &initFile2, - const bool displayHelp = false); - virtual void - initClick(const std::map *> - &mapOfImages, - const std::map &mapOfInitFiles, - const bool displayHelp = false); + virtual void initClick(const vpImage &I1, const vpImage &I2, + const std::string &initFile1, const std::string &initFile2, const bool displayHelp = false); + virtual void initClick(const std::map *> &mapOfImages, + const std::map &mapOfInitFiles, const bool displayHelp = false); #endif using vpMbTracker::initFromPoints; - virtual void initFromPoints(const vpImage &I1, - const vpImage &I2, - const std::string &initFile1, - const std::string &initFile2); - virtual void - initFromPoints(const std::map *> - &mapOfImages, - const std::map &mapOfInitPoints); + virtual void initFromPoints(const vpImage &I1, const vpImage &I2, + const std::string &initFile1, const std::string &initFile2); + virtual void initFromPoints(const std::map *> &mapOfImages, + const std::map &mapOfInitPoints); using vpMbTracker::initFromPose; - virtual void initFromPose(const vpImage &I1, - const vpImage &I2, - const std::string &initFile1, - const std::string &initFile2); - virtual void - initFromPose(const std::map *> - &mapOfImages, - const std::map &mapOfInitPoses); - - virtual void initFromPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo); - virtual void initFromPose( - const std::map *> - &mapOfImages, - const std::map &mapOfCameraPoses); + virtual void initFromPose(const vpImage &I1, const vpImage &I2, + const std::string &initFile1, const std::string &initFile2); + virtual void initFromPose(const std::map *> &mapOfImages, + const std::map &mapOfInitPoses); + + virtual void initFromPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo); + virtual void initFromPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses); virtual void loadConfigFile(const std::string &configFile); - virtual void loadConfigFile(const std::string &configFile1, - const std::string &configFile2); - virtual void - loadConfigFile(const std::map &mapOfConfigFiles); + virtual void loadConfigFile(const std::string &configFile1, const std::string &configFile2); + virtual void loadConfigFile(const std::map &mapOfConfigFiles); using vpMbTracker::loadModel; - virtual void loadModel(const std::string &modelFile, - const bool verbose = false); - virtual void loadModel(const std::string &modelFile1, - const std::string &modelFile2, - const bool verbose = false); - virtual void - loadModel(const std::map &mapOfModelFiles, - const bool verbose = false); - - virtual void reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, + virtual void loadModel(const std::string &modelFile, const bool verbose = false); + virtual void loadModel(const std::string &modelFile1, const std::string &modelFile2, const bool verbose = false); + virtual void loadModel(const std::map &mapOfModelFiles, const bool verbose = false); + + virtual void reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose = false); + virtual void reInitModel(const vpImage &I1, const vpImage &I2, + const std::string &cad_name1, const std::string &cad_name2, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const bool verbose = false); + virtual void reInitModel(const std::map *> &mapOfImages, + const std::map &mapOfModelFiles, + const std::map &mapOfCameraPoses, const bool verbose = false); - virtual void - reInitModel(const vpImage &I1, - const vpImage &I2, const std::string &cad_name1, - const std::string &cad_name2, const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, const bool verbose = false); - virtual void reInitModel( - const std::map *> - &mapOfImages, - const std::map &mapOfModelFiles, - const std::map &mapOfCameraPoses, - const bool verbose = false); virtual void resetTracker(); virtual void setAngleAppear(const double &a); virtual void setAngleAppear(const double &a1, const double &a2); - virtual void - setAngleAppear(const std::map &mapOfAngles); + virtual void setAngleAppear(const std::map &mapOfAngles); virtual void setAngleDisappear(const double &a); virtual void setAngleDisappear(const double &a1, const double &a2); - virtual void - setAngleDisappear(const std::map &mapOfAngles); + virtual void setAngleDisappear(const std::map &mapOfAngles); virtual void setCameraParameters(const vpCameraParameters &camera); - virtual void setCameraParameters(const vpCameraParameters &camera1, - const vpCameraParameters &camera2); - virtual void setCameraParameters( - const std::map &mapOfCameraParameters); - - virtual void setCameraTransformationMatrix( - const std::string &cameraName, - const vpHomogeneousMatrix &cameraTransformationMatrix); - virtual void setCameraTransformationMatrix( - const std::map - &mapOfTransformationMatrix); + virtual void setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2); + virtual void setCameraParameters(const std::map &mapOfCameraParameters); - virtual void setClipping(const unsigned int &flags); - virtual void setClipping(const unsigned int &flags1, - const unsigned int &flags2); + virtual void setCameraTransformationMatrix(const std::string &cameraName, + const vpHomogeneousMatrix &cameraTransformationMatrix); virtual void - setClipping(const std::map &mapOfClippingFlags); + setCameraTransformationMatrix(const std::map &mapOfTransformationMatrix); + + virtual void setClipping(const unsigned int &flags); + virtual void setClipping(const unsigned int &flags1, const unsigned int &flags2); + virtual void setClipping(const std::map &mapOfClippingFlags); virtual void setDepthDenseFilteringMaxDistance(const double maxDistance); virtual void setDepthDenseFilteringMethod(const int method); virtual void setDepthDenseFilteringMinDistance(const double minDistance); - virtual void - setDepthDenseFilteringOccupancyRatio(const double occupancyRatio); - virtual void setDepthDenseSamplingStep(const unsigned int stepX, - const unsigned int stepY); - - virtual void setDepthNormalFaceCentroidMethod( - const vpMbtFaceDepthNormal::vpFaceCentroidType &method); - virtual void setDepthNormalFeatureEstimationMethod( - const vpMbtFaceDepthNormal::vpFeatureEstimationType &method); + virtual void setDepthDenseFilteringOccupancyRatio(const double occupancyRatio); + virtual void setDepthDenseSamplingStep(const unsigned int stepX, const unsigned int stepY); + + virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method); + virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method); virtual void setDepthNormalPclPlaneEstimationMethod(const int method); - virtual void - setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter); - virtual void - setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold); - virtual void setDepthNormalSamplingStep(const unsigned int stepX, - const unsigned int stepY); + virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter); + virtual void setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold); + virtual void setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY); virtual void setDisplayFeatures(const bool displayF); virtual void setFarClippingDistance(const double &dist); - virtual void setFarClippingDistance(const double &dist1, - const double &dist2); - virtual void setFarClippingDistance( - const std::map &mapOfClippingDists); + virtual void setFarClippingDistance(const double &dist1, const double &dist2); + virtual void setFarClippingDistance(const std::map &mapOfClippingDists); - virtual void setFeatureFactors( - const std::map &mapOfFeatureFactors); + virtual void setFeatureFactors(const std::map &mapOfFeatureFactors); virtual void setGoodMovingEdgesRatioThreshold(const double threshold); #ifdef VISP_HAVE_OGRE virtual void setGoodNbRayCastingAttemptsRatio(const double &ratio); - virtual void - setNbRayCastingAttemptsForVisibility(const unsigned int &attempts); + virtual void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts); #endif -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) virtual void setKltMaskBorder(const unsigned int &e); - virtual void setKltMaskBorder(const unsigned int &e1, - const unsigned int &e2); - virtual void - setKltMaskBorder(const std::map &mapOfErosions); + virtual void setKltMaskBorder(const unsigned int &e1, const unsigned int &e2); + virtual void setKltMaskBorder(const std::map &mapOfErosions); virtual void setKltOpencv(const vpKltOpencv &t); virtual void setKltOpencv(const vpKltOpencv &t1, const vpKltOpencv &t2); - virtual void - setKltOpencv(const std::map &mapOfKlts); + virtual void setKltOpencv(const std::map &mapOfKlts); virtual void setKltThresholdAcceptation(const double th); @@ -372,36 +274,27 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker 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 setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name = ""); + virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name = ""); virtual void setMovingEdge(const vpMe &me); virtual void setMovingEdge(const vpMe &me1, const vpMe &me2); virtual void setMovingEdge(const std::map &mapOfMe); virtual void setNearClippingDistance(const double &dist); - virtual void setNearClippingDistance(const double &dist1, - const double &dist2); - virtual void - setNearClippingDistance(const std::map &mapOfDists); + virtual void setNearClippingDistance(const double &dist1, const double &dist2); + virtual void setNearClippingDistance(const std::map &mapOfDists); virtual void setOgreShowConfigDialog(const bool showConfigDialog); virtual void setOgreVisibilityTest(const bool &v); virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt); - virtual void setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo); - virtual void setPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo); - virtual void - setPose(const std::map *> - &mapOfImages, - const std::map &mapOfCameraPoses); + virtual void setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo); + virtual void setPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo); + virtual void setPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses); virtual void setProjectionErrorComputation(const bool &flag); @@ -410,59 +303,44 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual void setScanLineVisibilityTest(const bool &v); virtual void setTrackerType(const int type); - virtual void - setTrackerType(const std::map &mapOfTrackerTypes); - - virtual void setUseEdgeTracking(const std::string &name, - const bool &useEdgeTracking); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) - virtual void setUseKltTracking(const std::string &name, - const bool &useKltTracking); + virtual void setTrackerType(const std::map &mapOfTrackerTypes); + + virtual void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking); +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) + virtual void setUseKltTracking(const std::string &name, const bool &useKltTracking); #endif virtual void testTracking(); virtual void track(const vpImage &I); - virtual void track(const vpImage &I1, - const vpImage &I2); - virtual void - track(std::map *> &mapOfImages); + virtual void track(const vpImage &I1, const vpImage &I2); + virtual void track(std::map *> &mapOfImages); #ifdef VISP_HAVE_PCL - virtual void - track(std::map *> &mapOfImages, - std::map::ConstPtr> - &mapOfPointClouds); + virtual void track(std::map *> &mapOfImages, + std::map::ConstPtr> &mapOfPointClouds); #endif - virtual void - track(std::map *> &mapOfImages, - std::map *> - &mapOfPointClouds, - std::map &mapOfPointCloudWidths, - std::map &mapOfPointCloudHeights); + virtual void track(std::map *> &mapOfImages, + std::map *> &mapOfPointClouds, + std::map &mapOfPointCloudWidths, + std::map &mapOfPointCloudHeights); protected: virtual void computeProjectionError(); - virtual void computeVVS( - std::map *> &mapOfImages); + virtual void computeVVS(std::map *> &mapOfImages); virtual void computeVVSInit(); - virtual void computeVVSInit( - std::map *> &mapOfImages); + virtual void computeVVSInit(std::map *> &mapOfImages); virtual void computeVVSInteractionMatrixAndResidu(); - virtual void computeVVSInteractionMatrixAndResidu( - std::map *> &mapOfImages, - std::map &mapOfVelocityTwist); + virtual void computeVVSInteractionMatrixAndResidu(std::map *> &mapOfImages, + std::map &mapOfVelocityTwist); using vpMbTracker::computeVVSWeights; virtual void computeVVSWeights(); - virtual void initCircle(const vpPoint &p1, const vpPoint &p2, - const vpPoint &p3, const double radius, + 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, + 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); @@ -470,22 +348,17 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual void initFaceFromLines(vpMbtPolygon &polygon); #ifdef VISP_HAVE_PCL - virtual void preTracking( - std::map *> &mapOfImages, - std::map::ConstPtr> - &mapOfPointClouds); + virtual void preTracking(std::map *> &mapOfImages, + std::map::ConstPtr> &mapOfPointClouds); #endif - virtual void preTracking( - std::map *> &mapOfImages, - std::map *> - &mapOfPointClouds, - std::map &mapOfPointCloudWidths, - std::map &mapOfPointCloudHeights); + virtual void preTracking(std::map *> &mapOfImages, + std::map *> &mapOfPointClouds, + std::map &mapOfPointCloudWidths, + std::map &mapOfPointCloudHeights); private: class TrackerWrapper : public vpMbEdgeTracker, -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) public vpMbKltTracker, #endif public vpMbDepthNormalTracker, @@ -516,25 +389,17 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual inline int getTrackerType() const { return m_trackerType; } - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); virtual void init(const vpImage &I); virtual void loadConfigFile(const std::string &configFile); - virtual void reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose = false); + virtual void reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose = false); virtual void resetTracker(); @@ -548,8 +413,7 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual void setOgreVisibilityTest(const bool &v); - virtual void setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo); + virtual void setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo); virtual void setProjectionErrorComputation(const bool &flag); @@ -564,8 +428,7 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker using vpMbDepthNormalTracker::track; using vpMbDepthDenseTracker::track; virtual void track(const vpImage *const ptr_I = NULL, - const pcl::PointCloud::ConstPtr - &point_cloud = nullptr); + const pcl::PointCloud::ConstPtr &point_cloud = nullptr); #endif protected: @@ -574,18 +437,14 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual void computeVVSInit(const vpImage *const ptr_I); virtual void computeVVSInteractionMatrixAndResidu(); using vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu; - virtual void computeVVSInteractionMatrixAndResidu( - const vpImage *const ptr_I); + virtual void computeVVSInteractionMatrixAndResidu(const vpImage *const ptr_I); using vpMbTracker::computeVVSWeights; virtual void computeVVSWeights(); - 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 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, + 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); @@ -594,23 +453,16 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual void initMbtTracking(const vpImage *const ptr_I); #ifdef VISP_HAVE_PCL - virtual void - postTracking(const vpImage *const ptr_I = NULL, - const pcl::PointCloud::ConstPtr &point_cloud = - nullptr); + virtual void postTracking(const vpImage *const ptr_I = NULL, + const pcl::PointCloud::ConstPtr &point_cloud = nullptr); virtual void preTracking(const vpImage *const ptr_I = NULL, - const pcl::PointCloud::ConstPtr - &point_cloud = nullptr); + const pcl::PointCloud::ConstPtr &point_cloud = nullptr); #endif - virtual void - postTracking(const vpImage *const ptr_I = NULL, - const unsigned int pointcloud_width = 0, - const unsigned int pointcloud_height = 0); - virtual void - preTracking(const vpImage *const ptr_I = NULL, - const std::vector *const point_cloud = NULL, - const unsigned int pointcloud_width = 0, - const unsigned int pointcloud_height = 0); + virtual void postTracking(const vpImage *const ptr_I = NULL, const unsigned int pointcloud_width = 0, + const unsigned int pointcloud_height = 0); + virtual void preTracking(const vpImage *const ptr_I = NULL, + const std::vector *const point_cloud = NULL, + const unsigned int pointcloud_width = 0, const unsigned int pointcloud_height = 0); }; protected: @@ -620,8 +472,7 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker vpMatrix m_L; //! Map of camera transformation matrix between the current camera frame to //! the reference camera frame (cCurrent_M_cRef) - std::map - m_mapOfCameraTransformationMatrix; + std::map m_mapOfCameraTransformationMatrix; //! Ponderation between each feature type in the VVS stage std::map m_mapOfFeatureFactors; //! Map of Model-based trackers, key is the name of the camera, value is the diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h b/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h index c882db6124..3f724f7c70 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h @@ -57,9 +57,7 @@ template class vpMbHiddenFaces; -template -void swap(vpMbHiddenFaces &first, - vpMbHiddenFaces &second); +template void swap(vpMbHiddenFaces &first, vpMbHiddenFaces &second); /*! \class vpMbHiddenFaces @@ -88,12 +86,10 @@ template class vpMbHiddenFaces bool ogreShowConfigDialog; #endif - unsigned int setVisiblePrivate( - const vpHomogeneousMatrix &cMo, const double &angleAppears, - const double &angleDisappears, bool &changed, bool useOgre = false, - bool not_used = false, - const vpImage &I = vpImage(), - const vpCameraParameters &cam = vpCameraParameters()); + unsigned int setVisiblePrivate(const vpHomogeneousMatrix &cMo, const double &angleAppears, + const double &angleDisappears, bool &changed, bool useOgre = false, + bool not_used = false, const vpImage &I = vpImage(), + const vpCameraParameters &cam = vpCameraParameters()); public: vpMbHiddenFaces(); @@ -104,23 +100,15 @@ template class vpMbHiddenFaces void addPolygon(PolygonType *p); - bool computeVisibility(const vpHomogeneousMatrix &cMo, - const double &angleAppears, - const double &angleDisappears, bool &changed, - bool useOgre, bool not_used, - const vpImage &I, - const vpCameraParameters &cam, - const vpTranslationVector &cameraPos, - unsigned int index); + bool computeVisibility(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, + bool &changed, bool useOgre, bool not_used, const vpImage &I, + const vpCameraParameters &cam, const vpTranslationVector &cameraPos, unsigned int index); - void computeClippedPolygons(const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam); + void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam); - void computeScanLineRender(const vpCameraParameters &cam, - const unsigned int &w, const unsigned int &h); + void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h); - void computeScanLineQuery(const vpPoint &a, const vpPoint &b, - std::vector > &lines, + void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector > &lines, const bool &displayResults = false); vpMbScanLine &getMbScanLineRenderer() { return scanlineRender; } @@ -157,10 +145,7 @@ template class vpMbHiddenFaces \return Number of rays sent. */ - unsigned int getNbRayCastingAttemptsForVisibility() - { - return nbRayAttempts; - } + unsigned int getNbRayCastingAttemptsForVisibility() { return nbRayAttempts; } /*! Get the Ogre3D Context. @@ -202,17 +187,13 @@ template class vpMbHiddenFaces bool isVisible(const unsigned int i) { return Lpol[i]->isVisible(); } #ifdef VISP_HAVE_OGRE - bool isVisibleOgre(const vpTranslationVector &cameraPos, - const unsigned int &index); + 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]; - } + inline const PolygonType *operator[](const unsigned int i) const { return Lpol[i]; } void reset(); @@ -241,10 +222,7 @@ template class vpMbHiddenFaces \param attempts Number of rays to be sent. */ - void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) - { - nbRayAttempts = attempts; - } + void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { nbRayAttempts = attempts; } /*! Set the ratio of visibility attempts that has to be successful to consider @@ -273,34 +251,23 @@ template class vpMbHiddenFaces Ogre rendering options) when Ogre visibility is enabled. By default, this functionality is turned off. */ - inline void setOgreShowConfigDialog(const bool showConfigDialog) - { - ogreShowConfigDialog = showConfigDialog; - } + inline void setOgreShowConfigDialog(const bool showConfigDialog) { ogreShowConfigDialog = showConfigDialog; } #endif - unsigned int setVisible(const vpImage &I, - const vpCameraParameters &cam, - const vpHomogeneousMatrix &cMo, const double &angle, + unsigned int setVisible(const vpImage &I, const vpCameraParameters &cam, + const vpHomogeneousMatrix &cMo, const double &angle, bool &changed); + unsigned int setVisible(const vpImage &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); - unsigned int setVisible(const vpImage &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 &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); + unsigned int setVisibleOgre(const vpImage &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. @@ -314,8 +281,7 @@ template class vpMbHiddenFaces Basic constructor. */ template -vpMbHiddenFaces::vpMbHiddenFaces() - : Lpol(), nbVisiblePolygon(0), scanlineRender() +vpMbHiddenFaces::vpMbHiddenFaces() : Lpol(), nbVisiblePolygon(0), scanlineRender() { #ifdef VISP_HAVE_OGRE ogreInitialised = false; @@ -362,16 +328,12 @@ template vpMbHiddenFaces::~vpMbHiddenFaces() Copy constructor. */ template -vpMbHiddenFaces::vpMbHiddenFaces( - const vpMbHiddenFaces ©) - : Lpol(), nbVisiblePolygon(copy.nbVisiblePolygon), - scanlineRender(copy.scanlineRender) +vpMbHiddenFaces::vpMbHiddenFaces(const vpMbHiddenFaces ©) + : Lpol(), nbVisiblePolygon(copy.nbVisiblePolygon), scanlineRender(copy.scanlineRender) #ifdef VISP_HAVE_OGRE , - ogreBackground(copy.ogreBackground), - ogreInitialised(copy.ogreInitialised), nbRayAttempts(copy.nbRayAttempts), - ratioVisibleRay(copy.ratioVisibleRay), ogre(NULL), lOgrePolygons(), - ogreShowConfigDialog(copy.ogreShowConfigDialog) + ogreBackground(copy.ogreBackground), ogreInitialised(copy.ogreInitialised), nbRayAttempts(copy.nbRayAttempts), + ratioVisibleRay(copy.ratioVisibleRay), ogre(NULL), lOgrePolygons(), ogreShowConfigDialog(copy.ogreShowConfigDialog) #endif { // Copy the list of polygons @@ -381,9 +343,7 @@ vpMbHiddenFaces::vpMbHiddenFaces( } } -template -void swap(vpMbHiddenFaces &first, - vpMbHiddenFaces &second) +template void swap(vpMbHiddenFaces &first, vpMbHiddenFaces &second) { using std::swap; swap(first.Lpol, second.Lpol); @@ -403,8 +363,7 @@ void swap(vpMbHiddenFaces &first, Copy assignment operator. */ template -vpMbHiddenFaces &vpMbHiddenFaces:: -operator=(vpMbHiddenFaces other) +vpMbHiddenFaces &vpMbHiddenFaces::operator=(vpMbHiddenFaces other) { swap(*this, other); @@ -416,8 +375,7 @@ operator=(vpMbHiddenFaces other) \param p : The polygon to add. */ -template -void vpMbHiddenFaces::addPolygon(PolygonType *p) +template void vpMbHiddenFaces::addPolygon(PolygonType *p) { PolygonType *p_new = new PolygonType; p_new->index = p->index; @@ -480,8 +438,7 @@ template void vpMbHiddenFaces::reset() \param cam : Camera parameters that will be used to clip the polygons. */ template -void vpMbHiddenFaces::computeClippedPolygons( - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam) +void vpMbHiddenFaces::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. @@ -504,14 +461,11 @@ void vpMbHiddenFaces::computeClippedPolygons( \param h : Height of the render window. */ template -void vpMbHiddenFaces::computeScanLineRender( - const vpCameraParameters &cam, const unsigned int &w, - const unsigned int &h) +void vpMbHiddenFaces::computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, + const unsigned int &h) { - std::vector > > polyClipped( - Lpol.size()); - std::vector > *> - listPolyClipped; + std::vector > > polyClipped(Lpol.size()); + std::vector > *> listPolyClipped; std::vector listPolyIndices; for (unsigned int i = 0; i < Lpol.size(); i++) { @@ -544,10 +498,9 @@ void vpMbHiddenFaces::computeScanLineRender( displayed. False otherwise */ template -void vpMbHiddenFaces::computeScanLineQuery( - const vpPoint &a, const vpPoint &b, - std::vector > &lines, - const bool &displayResults) +void vpMbHiddenFaces::computeScanLineQuery(const vpPoint &a, const vpPoint &b, + std::vector > &lines, + const bool &displayResults) { scanlineRender.queryLineVisibility(a, b, lines, displayResults); } @@ -567,10 +520,10 @@ void vpMbHiddenFaces::computeScanLineQuery( \return Return the number of visible polygons */ template -unsigned int vpMbHiddenFaces::setVisiblePrivate( - const vpHomogeneousMatrix &cMo, const double &angleAppears, - const double &angleDisappears, bool &changed, bool useOgre, bool not_used, - const vpImage &I, const vpCameraParameters &cam) +unsigned int vpMbHiddenFaces::setVisiblePrivate(const vpHomogeneousMatrix &cMo, const double &angleAppears, + const double &angleDisappears, bool &changed, bool useOgre, + bool not_used, const vpImage &I, + const vpCameraParameters &cam) { nbVisiblePolygon = 0; changed = false; @@ -588,8 +541,7 @@ unsigned int vpMbHiddenFaces::setVisiblePrivate( for (unsigned int i = 0; i < Lpol.size(); i++) { // std::cout << "Calling poly: " << i << std::endl; - if (computeVisibility(cMo, angleAppears, angleDisappears, changed, - useOgre, not_used, I, cam, cameraPos, i)) + if (computeVisibility(cMo, angleAppears, angleDisappears, changed, useOgre, not_used, I, cam, cameraPos, i)) nbVisiblePolygon++; } return nbVisiblePolygon; @@ -612,11 +564,11 @@ unsigned int vpMbHiddenFaces::setVisiblePrivate( \return Return true if the face is visible. */ template -bool vpMbHiddenFaces::computeVisibility( - const vpHomogeneousMatrix &cMo, const double &angleAppears, - const double &angleDisappears, bool &changed, bool useOgre, bool not_used, - const vpImage &I, const vpCameraParameters &cam, - const vpTranslationVector &cameraPos, unsigned int index) +bool vpMbHiddenFaces::computeVisibility(const vpHomogeneousMatrix &cMo, const double &angleAppears, + const double &angleDisappears, bool &changed, bool useOgre, + bool not_used, const vpImage &I, + const vpCameraParameters &cam, + const vpTranslationVector &cameraPos, unsigned int index) { (void)not_used; unsigned int i = index; @@ -637,19 +589,15 @@ bool vpMbHiddenFaces::computeVisibility( if (!testDisappear) { if (useOgre) #ifdef VISP_HAVE_OGRE - testDisappear = - ((!Lpol[i]->isVisible(cMo, angleDisappears, true, cam, I)) || - !isVisibleOgre(cameraPos, i)); + testDisappear = ((!Lpol[i]->isVisible(cMo, angleDisappears, true, cam, I)) || !isVisibleOgre(cameraPos, i)); #else { (void)cameraPos; // Avoid warning - testDisappear = - (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I)); + testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I)); } #endif else - testDisappear = - (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I)); + testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I)); } // test if the face is still visible @@ -671,9 +619,7 @@ bool vpMbHiddenFaces::computeVisibility( if (testAppear) { if (useOgre) #ifdef VISP_HAVE_OGRE - testAppear = - ((Lpol[i]->isVisible(cMo, angleAppears, true, cam, I)) && - isVisibleOgre(cameraPos, i)); + testAppear = ((Lpol[i]->isVisible(cMo, angleAppears, true, cam, I)) && isVisibleOgre(cameraPos, i)); #else testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, I)); #endif @@ -709,9 +655,9 @@ bool vpMbHiddenFaces::computeVisibility( \return Return the number of visible polygons */ template -unsigned int vpMbHiddenFaces::setVisible( - const vpImage &I, const vpCameraParameters &cam, - const vpHomogeneousMatrix &cMo, const double &angle, bool &changed) +unsigned int vpMbHiddenFaces::setVisible(const vpImage &I, const vpCameraParameters &cam, + const vpHomogeneousMatrix &cMo, const double &angle, + bool &changed) { return setVisible(I, cam, cMo, angle, angle, changed); } @@ -729,13 +675,11 @@ unsigned int vpMbHiddenFaces::setVisible( \return Return the number of visible polygons */ template -unsigned int vpMbHiddenFaces::setVisible( - const vpImage &I, const vpCameraParameters &cam, - const vpHomogeneousMatrix &cMo, const double &angleAppears, - const double &angleDisappears, bool &changed) +unsigned int vpMbHiddenFaces::setVisible(const vpImage &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); + return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, false, true, I, cam); } /*! @@ -750,12 +694,10 @@ unsigned int vpMbHiddenFaces::setVisible( \return Return the number of visible polygons */ template -unsigned int vpMbHiddenFaces::setVisible( - const vpHomogeneousMatrix &cMo, const double &angleAppears, - const double &angleDisappears, bool &changed) +unsigned int vpMbHiddenFaces::setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears, + const double &angleDisappears, bool &changed) { - return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, - false); + return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, false); } #ifdef VISP_HAVE_OGRE @@ -764,8 +706,7 @@ unsigned int vpMbHiddenFaces::setVisible( \param cam : Camera parameters. */ -template -void vpMbHiddenFaces::initOgre(const vpCameraParameters &cam) +template void vpMbHiddenFaces::initOgre(const vpCameraParameters &cam) { ogreInitialised = true; ogre->setCameraParameters(cam); @@ -773,14 +714,11 @@ void vpMbHiddenFaces::initOgre(const vpCameraParameters &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)); + Ogre::ManualObject *manual = ogre->getSceneManager()->createManualObject(Ogre::StringConverter::toString(n)); - manual->begin("BaseWhiteNoLighting", - Ogre::RenderOperation::OT_LINE_STRIP); + 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(), + 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); @@ -789,10 +727,7 @@ void vpMbHiddenFaces::initOgre(const vpCameraParameters &cam) manual->index(0); manual->end(); - ogre->getSceneManager() - ->getRootSceneNode() - ->createChildSceneNode() - ->attachObject(manual); + ogre->getSceneManager()->getRootSceneNode()->createChildSceneNode()->attachObject(manual); lOgrePolygons.push_back(manual); } @@ -803,8 +738,7 @@ void vpMbHiddenFaces::initOgre(const vpCameraParameters &cam) \param cMo : Pose used to display. */ -template -void vpMbHiddenFaces::displayOgre(const vpHomogeneousMatrix &cMo) +template void vpMbHiddenFaces::displayOgre(const vpHomogeneousMatrix &cMo) { if (ogreInitialised && !ogre->isWindowHidden()) { for (unsigned int i = 0; i < Lpol.size(); i++) { @@ -830,13 +764,12 @@ void vpMbHiddenFaces::displayOgre(const vpHomogeneousMatrix &cMo) \return Return the number of visible polygons */ template -unsigned int vpMbHiddenFaces::setVisibleOgre( - const vpImage &I, const vpCameraParameters &cam, - const vpHomogeneousMatrix &cMo, const double &angleAppears, - const double &angleDisappears, bool &changed) +unsigned int vpMbHiddenFaces::setVisibleOgre(const vpImage &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); + return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, true, true, I, cam); } /*! @@ -851,9 +784,8 @@ unsigned int vpMbHiddenFaces::setVisibleOgre( \return Return the number of visible polygons */ template -unsigned int vpMbHiddenFaces::setVisibleOgre( - const vpHomogeneousMatrix &cMo, const double &angleAppears, - const double &angleDisappears, bool &changed) +unsigned int vpMbHiddenFaces::setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears, + const double &angleDisappears, bool &changed) { return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, true); } @@ -867,11 +799,9 @@ unsigned int vpMbHiddenFaces::setVisibleOgre( \return Return true if the polygon is visible, False otherwise. */ template -bool vpMbHiddenFaces::isVisibleOgre( - const vpTranslationVector &cameraPos, const unsigned int &index) +bool vpMbHiddenFaces::isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index) { - Ogre::Vector3 camera((Ogre::Real)cameraPos[0], (Ogre::Real)cameraPos[1], - (Ogre::Real)cameraPos[2]); + 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; @@ -896,8 +826,7 @@ bool vpMbHiddenFaces::isVisibleOgre( 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::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; @@ -910,8 +839,7 @@ bool vpMbHiddenFaces::isVisibleOgre( Ogre::Real distanceCollision = direction.length(); direction.normalise(); - Ogre::RaySceneQuery *mRaySceneQuery = - ogre->getSceneManager()->createRayQuery(Ogre::Ray(camera, direction)); + Ogre::RaySceneQuery *mRaySceneQuery = ogre->getSceneManager()->createRayQuery(Ogre::Ray(camera, direction)); mRaySceneQuery->setSortByDistance(true); Ogre::RaySceneQueryResult &result = mRaySceneQuery->execute(); @@ -934,27 +862,22 @@ bool vpMbHiddenFaces::isVisibleOgre( // 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::fabs(Lpol[index]->getPoint(0).get_oX() - Lpol[index]->getPoint(1).get_oX()) < std::numeric_limits::epsilon()) + - (std::fabs(Lpol[index]->getPoint(0).get_oY() - - Lpol[index]->getPoint(1).get_oY()) < + (std::fabs(Lpol[index]->getPoint(0).get_oY() - Lpol[index]->getPoint(1).get_oY()) < std::numeric_limits::epsilon()) + - (std::fabs(Lpol[index]->getPoint(0).get_oZ() - - Lpol[index]->getPoint(1).get_oZ()) < + (std::fabs(Lpol[index]->getPoint(0).get_oZ() - Lpol[index]->getPoint(1).get_oZ()) < std::numeric_limits::epsilon())) >= 2)) { if (it != result.end()) { - if (it->movable->getName() == - Ogre::StringConverter::toString(index)) { + 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::epsilon()*/) + if (distance > distanceCollision || std::fabs(distance - distanceCollision) < + 1e-6 /*std::fabs(distance) * std::numeric_limits::epsilon()*/) nbVisible++; } } else @@ -967,8 +890,7 @@ bool vpMbHiddenFaces::isVisibleOgre( // std::cout << "For " << Ogre::StringConverter::toString(index) << ": // " << it->movable->getName() << " / " << std::flush; - if (it->movable->getName() == - Ogre::StringConverter::toString(index)) { + if (it->movable->getName() == Ogre::StringConverter::toString(index)) { nbVisible++; } else { ++it; @@ -978,8 +900,7 @@ bool vpMbHiddenFaces::isVisibleOgre( if (std::fabs(distance - distancePrev) < 1e-6 /*std::fabs(distance) * std::numeric_limits::epsilon()*/) { // std::cout << it->movable->getName() << " / " << std::flush; - if (it->movable->getName() == - Ogre::StringConverter::toString(index)) { + if (it->movable->getName() == Ogre::StringConverter::toString(index)) { nbVisible++; break; } @@ -996,8 +917,7 @@ bool vpMbHiddenFaces::isVisibleOgre( } if (((double)nbVisible) / ((double)nbRayAttempts) > ratioVisibleRay || - std::fabs(((double)nbVisible) / ((double)nbRayAttempts) - - ratioVisibleRay) < + std::fabs(((double)nbVisible) / ((double)nbRayAttempts) - ratioVisibleRay) < ratioVisibleRay * std::numeric_limits::epsilon()) visible = true; else diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbKltMultiTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbKltMultiTracker.h index 6be449613a..1278ec151c 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbKltMultiTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbKltMultiTracker.h @@ -51,8 +51,7 @@ #include -#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && \ - (VISP_HAVE_OPENCV_VERSION >= 0x020100) +#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100) #include @@ -76,8 +75,7 @@ class VISP_EXPORT vpMbKltMultiTracker : public vpMbKltTracker protected: //! Map of camera transformation matrix between the current camera frame to //! the reference camera frame (cCurrent_M_cRef) - std::map - m_mapOfCameraTransformationMatrix; + std::map m_mapOfCameraTransformationMatrix; //! Map of Model-based klt trackers std::map m_mapOfKltTrackers; @@ -102,94 +100,65 @@ class VISP_EXPORT vpMbKltMultiTracker : public vpMbKltTracker /** @name Inherited functionalities from vpMbKltMultiTracker */ //@{ - virtual void addCircle(const vpPoint &P1, const vpPoint &P2, - const vpPoint &P3, const double r, + virtual void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name = ""); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); - virtual void - display(const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, const vpCameraParameters &cam2, - const vpColor &color, const unsigned int thickness = 1, - const bool displayFullModel = false); - - virtual void display(const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, - const vpCameraParameters &cam2, const vpColor &color, - const unsigned int thickness = 1, + virtual void display(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1, + const vpCameraParameters &cam2, const vpColor &color, const unsigned int thickness = 1, const bool displayFullModel = false); - virtual void display( - const std::map *> - &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - - virtual void display( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); + virtual void display(const vpImage &I1, const vpImage &I2, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1, const vpCameraParameters &cam2, + const vpColor &color, const unsigned int thickness = 1, const bool displayFullModel = false); + + virtual void display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, const vpColor &col, + const unsigned int thickness = 1, const bool displayFullModel = false); + + virtual void display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, const vpColor &col, + const unsigned int thickness = 1, const bool displayFullModel = false); virtual std::vector getCameraNames() const; virtual void getCameraParameters(vpCameraParameters &camera) const; - virtual void getCameraParameters(vpCameraParameters &cam1, - vpCameraParameters &cam2) const; - virtual void getCameraParameters(const std::string &cameraName, - vpCameraParameters &camera) const; - virtual void getCameraParameters( - std::map &mapOfCameraParameters) const; + virtual void getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const; + virtual void getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const; + virtual void getCameraParameters(std::map &mapOfCameraParameters) const; using vpMbTracker::getClipping; virtual unsigned int getClipping(const std::string &cameraName) const; virtual vpMbHiddenFaces &getFaces(); - virtual vpMbHiddenFaces & - getFaces(const std::string &cameraName); - virtual std::map > - getFaces() const; + virtual vpMbHiddenFaces &getFaces(const std::string &cameraName); + virtual std::map > getFaces() const; virtual std::list &getFeaturesCircle(); - virtual std::list & - getFeaturesCircle(const std::string &cameraName); + virtual std::list &getFeaturesCircle(const std::string &cameraName); virtual std::list &getFeaturesKlt(); - virtual std::list & - getFeaturesKlt(const std::string &cameraName); + virtual std::list &getFeaturesKlt(const std::string &cameraName); virtual std::list &getFeaturesKltCylinder(); - virtual std::list & - getFeaturesKltCylinder(const std::string &cameraName); + virtual std::list &getFeaturesKltCylinder(const std::string &cameraName); - virtual std::map > - getKltImagePoints() const; + virtual std::map > getKltImagePoints() const; - virtual std::map > - getKltImagePointsWithId() const; + virtual std::map > getKltImagePointsWithId() const; virtual std::map getKltOpencv() const; #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) - virtual std::map > - getKltPoints() const; + virtual std::map > getKltPoints() const; #else virtual std::map getKltPoints(); #endif @@ -204,18 +173,12 @@ class VISP_EXPORT vpMbKltMultiTracker : public vpMbKltTracker \return The number of cameras. */ - inline unsigned int getNumberOfCameras() const - { - return (unsigned int)m_mapOfKltTrackers.size(); - } + inline unsigned int getNumberOfCameras() const { return (unsigned int)m_mapOfKltTrackers.size(); } using vpMbTracker::getPose; - virtual void getPose(vpHomogeneousMatrix &c1Mo, - vpHomogeneousMatrix &c2Mo) const; - virtual void getPose(const std::string &cameraName, - vpHomogeneousMatrix &cMo_) const; - virtual void - getPose(std::map &mapOfCameraPoses) const; + virtual void getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const; + virtual void getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo_) const; + virtual void getPose(std::map &mapOfCameraPoses) const; virtual inline vpColVector getError() const { return m_error_kltMulti; } @@ -224,83 +187,53 @@ class VISP_EXPORT vpMbKltMultiTracker : public vpMbKltTracker virtual void init(const vpImage &I); #ifdef VISP_HAVE_MODULE_GUI - virtual void initClick(const vpImage &I, - const std::vector &points3D_list, + virtual void initClick(const vpImage &I, const std::vector &points3D_list, const std::string &displayFile = ""); - virtual void initClick(const vpImage &I, - const std::string &initFile, - const bool displayHelp = false); + virtual void initClick(const vpImage &I, const std::string &initFile, const bool displayHelp = false); - virtual void initClick(const vpImage &I1, - const vpImage &I2, - const std::string &initFile1, - const std::string &initFile2, - const bool displayHelp = false, + virtual void initClick(const vpImage &I1, const vpImage &I2, + const std::string &initFile1, const std::string &initFile2, const bool displayHelp = false, const bool firstCameraIsReference = true); - virtual void - initClick(const std::map *> - &mapOfImages, - const std::string &initFile, const bool displayHelp = false); + virtual void initClick(const std::map *> &mapOfImages, + const std::string &initFile, const bool displayHelp = false); - virtual void - initClick(const std::map *> - &mapOfImages, - const std::map &mapOfInitFiles, - const bool displayHelp = false); + virtual void initClick(const std::map *> &mapOfImages, + const std::map &mapOfInitFiles, const bool displayHelp = false); #endif - virtual void initFromPose(const vpImage &I, - const std::string &initFile); - virtual void initFromPose(const vpImage &I, - const vpHomogeneousMatrix &cMo); - virtual void initFromPose(const vpImage &I, - const vpPoseVector &cPo); - - virtual void initFromPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, + virtual void initFromPose(const vpImage &I, const std::string &initFile); + virtual void initFromPose(const vpImage &I, const vpHomogeneousMatrix &cMo); + virtual void initFromPose(const vpImage &I, const vpPoseVector &cPo); + + virtual void initFromPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference = true); - virtual void - initFromPose(const std::map *> - &mapOfImages, - const vpHomogeneousMatrix &cMo_); - virtual void initFromPose( - const std::map *> - &mapOfImages, - const std::map &mapOfCameraPoses); + virtual void initFromPose(const std::map *> &mapOfImages, + const vpHomogeneousMatrix &cMo_); + virtual void initFromPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses); virtual void loadConfigFile(const std::string &configFile); - virtual void loadConfigFile(const std::string &configFile1, - const std::string &configFile2, + virtual void loadConfigFile(const std::string &configFile1, const std::string &configFile2, const bool firstCameraIsReference = true); - virtual void - loadConfigFile(const std::map &mapOfConfigFiles); + virtual void loadConfigFile(const std::map &mapOfConfigFiles); - virtual void loadModel(const std::string &modelFile, - const bool verbose = false); + virtual void loadModel(const std::string &modelFile, const bool verbose = false); - virtual void reInitModel(const vpImage &I, + virtual void reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose = false); + virtual void reInitModel(const vpImage &I1, const vpImage &I2, + const std::string &cad_name, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const bool verbose = false, + const bool firstCameraIsReference = true); + virtual void reInitModel(const std::map *> &mapOfImages, const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, + const std::map &mapOfCameraPoses, const bool verbose = false); - virtual void reInitModel(const vpImage &I1, - const vpImage &I2, - const std::string &cad_name, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, - const bool verbose = false, - const bool firstCameraIsReference = true); - virtual void reInitModel( - const std::map *> - &mapOfImages, - const std::string &cad_name, - const std::map &mapOfCameraPoses, - const bool verbose = false); virtual void resetTracker(); @@ -309,35 +242,28 @@ class VISP_EXPORT vpMbKltMultiTracker : public vpMbKltTracker virtual void setCameraParameters(const vpCameraParameters &camera); - virtual void setCameraParameters(const vpCameraParameters &camera1, - const vpCameraParameters &camera2, + virtual void setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2, const bool firstCameraIsReference = true); - virtual void setCameraParameters(const std::string &cameraName, - const vpCameraParameters &camera); + virtual void setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera); - virtual void setCameraParameters( - const std::map &mapOfCameraParameters); + virtual void setCameraParameters(const std::map &mapOfCameraParameters); - virtual void setCameraTransformationMatrix( - const std::string &cameraName, - const vpHomogeneousMatrix &cameraTransformationMatrix); + virtual void setCameraTransformationMatrix(const std::string &cameraName, + const vpHomogeneousMatrix &cameraTransformationMatrix); - virtual void setCameraTransformationMatrix( - const std::map - &mapOfTransformationMatrix); + virtual void + setCameraTransformationMatrix(const std::map &mapOfTransformationMatrix); virtual void setClipping(const unsigned int &flags); - virtual void setClipping(const std::string &cameraName, - const unsigned int &flags); + virtual void setClipping(const std::string &cameraName, const unsigned int &flags); virtual void setCovarianceComputation(const bool &flag); virtual void setDisplayFeatures(const bool displayF); virtual void setFarClippingDistance(const double &dist); - virtual void setFarClippingDistance(const std::string &cameraName, - const double &dist); + virtual void setFarClippingDistance(const std::string &cameraName, const double &dist); #ifdef VISP_HAVE_OGRE void setGoodNbRayCastingAttemptsRatio(const double &ratio); @@ -348,27 +274,21 @@ class VISP_EXPORT vpMbKltMultiTracker : public vpMbKltTracker virtual void setKltMaskBorder(const unsigned int &e); virtual void setKltOpencv(const vpKltOpencv &t); - virtual void - setKltOpencv(const std::map &mapOfOpenCVTrackers); + virtual void setKltOpencv(const std::map &mapOfOpenCVTrackers); virtual void setKltThresholdAcceptation(const double th); virtual void setLod(const bool useLod, const std::string &name = ""); - virtual void setLod(const bool useLod, const std::string &cameraName, - const std::string &name); + virtual void setLod(const bool useLod, const std::string &cameraName, const std::string &name); - virtual void setMinLineLengthThresh(const double minLineLengthThresh, - 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 setMinPolygonAreaThresh(const double minPolygonAreaThresh, - const std::string &cameraName, + virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name = ""); + virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName, const std::string &name); virtual void setNearClippingDistance(const double &dist); - virtual void setNearClippingDistance(const std::string &cameraName, - const double &dist); + virtual void setNearClippingDistance(const std::string &cameraName, const double &dist); virtual void setOgreShowConfigDialog(const bool showConfigDialog); @@ -376,45 +296,34 @@ class VISP_EXPORT vpMbKltMultiTracker : public vpMbKltTracker virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt); - virtual void setPose(const vpImage &I, - const vpHomogeneousMatrix &cMo); + virtual void setPose(const vpImage &I, const vpHomogeneousMatrix &cMo); - virtual void setPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, + virtual void setPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference = true); - virtual void - setPose(const std::map *> - &mapOfImages, - const vpHomogeneousMatrix &cMo_); + virtual void setPose(const std::map *> &mapOfImages, + const vpHomogeneousMatrix &cMo_); - virtual void - setPose(const std::map *> - &mapOfImages, - const std::map &mapOfCameraPoses); + virtual void setPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses); virtual void setReferenceCameraName(const std::string &referenceCameraName); virtual void setScanLineVisibilityTest(const bool &v); - virtual void setUseKltTracking(const std::string &name, - const bool &useKltTracking); + virtual void setUseKltTracking(const std::string &name, const bool &useKltTracking); virtual void track(const vpImage &I); - virtual void track(const vpImage &I1, - const vpImage &I2); - virtual void - track(std::map *> &mapOfImages); + virtual void track(const vpImage &I1, const vpImage &I2); + virtual void track(std::map *> &mapOfImages); //@} /*! @name Deprecated functions */ //@{ - /* vp_deprecated */ virtual std::map - getNbKltPoints() const; + /* vp_deprecated */ virtual std::map getNbKltPoints() const; /* vp_deprecated */ virtual void setMaskBorder(const unsigned int &e); /* vp_deprecated */ virtual void setThresholdAcceptation(const double th); //@} @@ -425,16 +334,13 @@ class VISP_EXPORT vpMbKltMultiTracker : public vpMbKltTracker virtual void computeVVS(); virtual void computeVVSInit(); virtual void computeVVSInteractionMatrixAndResidu(); - virtual void computeVVSInteractionMatrixAndResidu( - std::map &mapOfVelocityTwist); + virtual void computeVVSInteractionMatrixAndResidu(std::map &mapOfVelocityTwist); virtual void computeVVSWeights(); using vpMbTracker::computeVVSWeights; - virtual void postTracking( - std::map *> &mapOfImages); + virtual void postTracking(std::map *> &mapOfImages); - virtual void preTracking( - std::map *> &mapOfImages); + virtual void preTracking(std::map *> &mapOfImages); using vpMbKltTracker::reinit; virtual void reinit(/* const vpImage& I */); diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbKltTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbKltTracker.h index 691e2401ce..09a7efec18 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbKltTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbKltTracker.h @@ -46,8 +46,7 @@ #include -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) #include #include @@ -107,12 +106,11 @@ int main() #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. + 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 + // Initialise manually the pose by clicking on the image points associated to the 3d points contained in the + // cube.init file. + tracker.initClick(I, "cube.init"); while(true){ // Acquire a new image @@ -120,8 +118,8 @@ cube.init file. 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); + tracker.display(I, cMo, cam, vpColor::darkRed, 1); // Display the model at the computed pose. + vpDisplay::flush(I); } #if defined VISP_HAVE_XML2 @@ -149,8 +147,7 @@ int main() #if defined VISP_HAVE_OPENCV vpMbKltTracker tracker; // Create a model based tracker via Klt Points. vpImage I; - vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then -computed using the tracker. + 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 @@ -158,8 +155,8 @@ computed using the tracker. #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. + // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used. + tracker.loadModel("cube.cao"); tracker.initFromPose(I, cMo); // initialize the tracker with the given pose. while(true){ @@ -169,8 +166,9 @@ is required, if coin is not installed .cao file can be used. } #if defined VISP_HAVE_XML2 - // Cleanup memory allocated by xml library used to parse the xml config file -in vpMbKltTracker::loadConfigFile() vpXmlParser::cleanup(); #endif + // Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile() + vpXmlParser::cleanup(); +#endif return 0; #endif @@ -207,22 +205,22 @@ int main() #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. + tracker.getCameraParameters(cam); // Get the camera parameters used by the tracker (from the configuration file). + // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used. + tracker.loadModel("cube.cao"); 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); + 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 + // Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile() + vpXmlParser::cleanup(); +#endif return 0; #endif @@ -287,34 +285,18 @@ class VISP_EXPORT vpMbKltTracker : public virtual vpMbTracker /** @name Inherited functionalities from vpMbKltTracker */ //@{ - void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, - const double r, const std::string &name = ""); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name = ""); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); /*! Return the address of the circle feature list. */ - virtual std::list &getFeaturesCircle() - { - return circles_disp; - } + virtual std::list &getFeaturesCircle() { return circles_disp; } /*! Return the address of the cylinder feature list. */ - virtual std::list &getFeaturesKltCylinder() - { - return kltCylinders; - } + virtual std::list &getFeaturesKltCylinder() { return kltCylinders; } /*! Return the address of the Klt feature list. */ - virtual std::list &getFeaturesKlt() - { - return kltPolygons; - } + virtual std::list &getFeaturesKlt() { return kltPolygons; } /*! Get the current list of KLT points. @@ -322,10 +304,7 @@ class VISP_EXPORT vpMbKltTracker : public virtual vpMbTracker \return the list of KLT points through vpKltOpencv. */ #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) - inline std::vector getKltPoints() const - { - return tracker.getFeatures(); - } + inline std::vector getKltPoints() const { return tracker.getFeatures(); } #else inline CvPoint2D32f *getKltPoints() { return tracker.getFeatures(); } #endif @@ -361,10 +340,7 @@ class VISP_EXPORT vpMbKltTracker : public virtual vpMbTracker \return threshold_outlier : Threshold for the weight below which a point is rejected. */ - inline double getKltThresholdAcceptation() const - { - return threshold_outlier; - } + inline double getKltThresholdAcceptation() const { return threshold_outlier; } virtual inline vpColVector getError() const { return m_error_klt; } @@ -373,12 +349,9 @@ class VISP_EXPORT vpMbKltTracker : public virtual vpMbTracker virtual void loadConfigFile(const std::string &configFile); void loadConfigFile(const char *configFile); - virtual void reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose = false); - void reInitModel(const vpImage &I, const char *cad_name, - const vpHomogeneousMatrix &cMo, + virtual void reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose = false); + void reInitModel(const vpImage &I, const char *cad_name, const vpHomogeneousMatrix &cMo, const bool verbose = false); void resetTracker(); @@ -403,10 +376,7 @@ class VISP_EXPORT vpMbKltTracker : public virtual vpMbTracker \param th : Threshold for the weight below which a point is rejected. */ - inline void setKltThresholdAcceptation(const double th) - { - threshold_outlier = th; - } + inline void setKltThresholdAcceptation(const double th) { threshold_outlier = th; } /*! Use Ogre3D for visibility tests @@ -433,14 +403,11 @@ class VISP_EXPORT vpMbKltTracker : public virtual vpMbTracker { vpMbTracker::setScanLineVisibilityTest(v); - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) (*it)->useScanLine = v; } - virtual void setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo); + virtual void setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo); /*! Set if the projection error criteria has to be computed. @@ -472,20 +439,14 @@ class VISP_EXPORT vpMbKltTracker : public virtual vpMbTracker \return The erosion. */ - /* vp_deprecated */ inline unsigned int getMaskBorder() const - { - return maskBorder; - } + /* vp_deprecated */ inline unsigned int getMaskBorder() const { return maskBorder; } /*! Get the current number of klt points. \deprecated Use rather getKltNbPoints() \return the number of features */ - /* vp_deprecated */ inline int getNbKltPoints() const - { - return tracker.getNbFeatures(); - } + /* vp_deprecated */ inline int getNbKltPoints() const { return tracker.getNbFeatures(); } /*! Get the threshold for the acceptation of a point. @@ -494,10 +455,7 @@ class VISP_EXPORT vpMbKltTracker : public virtual vpMbTracker \return threshold_outlier : Threshold for the weight below which a point is rejected. */ - /* vp_deprecated */ inline double getThresholdAcceptation() const - { - return threshold_outlier; - } + /* vp_deprecated */ inline double getThresholdAcceptation() const { return threshold_outlier; } /*! Set the erosion of the mask used on the Model faces. @@ -516,10 +474,7 @@ class VISP_EXPORT vpMbKltTracker : public virtual vpMbTracker \param th : Threshold for the weight below which a point is rejected. */ - /* vp_deprecated */ inline void setThresholdAcceptation(const double th) - { - threshold_outlier = th; - } + /* vp_deprecated */ inline void setThresholdAcceptation(const double th) { threshold_outlier = th; } //@} @@ -533,11 +488,9 @@ class VISP_EXPORT vpMbKltTracker : public virtual vpMbTracker virtual void init(const vpImage &I); virtual void initFaceFromCorners(vpMbtPolygon &polygon); virtual void initFaceFromLines(vpMbtPolygon &polygon); - virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, - const double, const int, + 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 = ""); + virtual void initCylinder(const vpPoint &, const vpPoint &, const double, const int, const std::string &name = ""); void preTracking(const vpImage &I); bool postTracking(const vpImage &I, vpColVector &w); diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbScanLine.h b/modules/tracker/mbt/include/visp3/mbt/vpMbScanLine.h index 5be6b9f7e1..add5519c54 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbScanLine.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbScanLine.h @@ -81,15 +81,11 @@ class VISP_EXPORT vpMbScanLine //! 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) - { - } + 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 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; @@ -98,8 +94,7 @@ class VISP_EXPORT vpMbScanLine //! vpMbScanLineEdge Comparator. struct vpMbScanLineEdgeComparator { - inline bool operator()(const vpMbScanLineEdge &l0, - const vpMbScanLineEdge &l1) const + 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]) @@ -117,18 +112,14 @@ class VISP_EXPORT vpMbScanLine //! vpMbScanLineSegment Comparators. struct vpMbScanLineSegmentComparator { - inline bool operator()(const vpMbScanLineSegment &a, - const vpMbScanLineSegment &b) const + 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::epsilon()) - ? a.type < b.type - : a.p < b.p; + return (std::fabs(a.p - b.p) <= std::numeric_limits::epsilon()) ? a.type < b.type : a.p < b.p; } - inline bool - operator()(const std::pair &a, - const std::pair &b) const + inline bool operator()(const std::pair &a, + const std::pair &b) const { return a.first < b.first; } @@ -140,8 +131,7 @@ class VISP_EXPORT vpMbScanLine unsigned int maskBorder; vpImage mask; vpImage primitive_ids; - std::map, vpMbScanLineEdgeComparator> - visibility_samples; + std::map, vpMbScanLineEdgeComparator> visibility_samples; double depthTreshold; public: @@ -154,11 +144,8 @@ class VISP_EXPORT vpMbScanLine vpMbScanLine(); ~vpMbScanLine(); - void drawScene( - const std::vector > *> - &polygons, - std::vector listPolyIndices, const vpCameraParameters &K, - unsigned int w, unsigned int h); + void drawScene(const std::vector > *> &polygons, + std::vector listPolyIndices, const vpCameraParameters &K, unsigned int w, unsigned int h); /*! If there is one polygon behind another, @@ -172,8 +159,7 @@ class VISP_EXPORT vpMbScanLine const vpImage &getMask() const { return mask; } const vpImage &getPrimitiveIDs() const { return primitive_ids; } - void queryLineVisibility(const vpPoint &a, const vpPoint &b, - std::vector > &lines, + void queryLineVisibility(const vpPoint &a, const vpPoint &b, std::vector > &lines, const bool &displayResults = false); /*! @@ -187,36 +173,26 @@ class VISP_EXPORT vpMbScanLine void setMaskBorder(const unsigned int &mb) { maskBorder = mb; } private: - void createScanLinesFromLocals( - std::vector > &scanlines, - std::vector > &localScanlines, - const unsigned int &size); + void createScanLinesFromLocals(std::vector > &scanlines, + std::vector > &localScanlines, + const unsigned int &size); - void drawLineY(const vpColVector &a, const vpColVector &b, - const vpMbScanLineEdge &line_ID, const int ID, + void drawLineY(const vpColVector &a, const vpColVector &b, const vpMbScanLineEdge &line_ID, const int ID, std::vector > &scanlines); - void drawLineX(const vpColVector &a, const vpColVector &b, - const vpMbScanLineEdge &line_ID, const int ID, + void drawLineX(const vpColVector &a, const vpColVector &b, const vpMbScanLineEdge &line_ID, const int ID, std::vector > &scanlines); - void - drawPolygonY(const std::vector > &polygon, - const int ID, - std::vector > &scanlines); + void drawPolygonY(const std::vector > &polygon, const int ID, + std::vector > &scanlines); - void - drawPolygonX(const std::vector > &polygon, - const int ID, - std::vector > &scanlines); + void drawPolygonX(const std::vector > &polygon, const int ID, + std::vector > &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 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); diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbTracker.h index cb48bbb082..090c4d3b50 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbTracker.h @@ -106,10 +106,7 @@ class VISP_EXPORT vpMbTracker { public: - typedef enum { - GAUSS_NEWTON_OPT = 0, - LEVENBERG_MARQUARDT_OPT = 1 - } vpMbtOptimizationMethod; + typedef enum { GAUSS_NEWTON_OPT = 0, LEVENBERG_MARQUARDT_OPT = 1 } vpMbtOptimizationMethod; protected: //! The camera parameters. @@ -215,10 +212,7 @@ class VISP_EXPORT vpMbTracker \param camera : copy of the camera parameters used by the tracker. */ - virtual void getCameraParameters(vpCameraParameters &camera) const - { - camera = this->cam; - } + virtual void getCameraParameters(vpCameraParameters &camera) const { camera = this->cam; } /*! Get the clipping used and defined in @@ -290,10 +284,11 @@ class VISP_EXPORT vpMbTracker 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; + 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() @@ -324,9 +319,9 @@ class VISP_EXPORT vpMbTracker for(unsigned int i=0; i(faces.size()); - } + virtual inline unsigned int getNbPolygon() const { return static_cast(faces.size()); } /*! Get the near distance for clipping. \return Near clipping value. */ - virtual inline double getNearClippingDistance() const - { - return distNearClip; - } + virtual inline double getNearClippingDistance() const { return distNearClip; } /*! Get the optimization method used during the tracking. @@ -359,10 +348,7 @@ class VISP_EXPORT vpMbTracker \return Optimization method. */ - virtual inline vpMbtOptimizationMethod getOptimizationMethod() const - { - return m_optimizationMethod; - } + virtual inline vpMbtOptimizationMethod getOptimizationMethod() const { return m_optimizationMethod; } /*! Return the polygon (face) "index". @@ -382,11 +368,8 @@ class VISP_EXPORT vpMbTracker return faces[index]; } - virtual std::pair, - std::vector > > - getPolygonFaces(const bool orderPolygons = true, - const bool useVisibility = true, - const bool clipPolygon = false); + virtual std::pair, std::vector > > + getPolygonFaces(const bool orderPolygons = true, const bool useVisibility = true, const bool clipPolygon = false); /*! Get the current pose between the object and the camera. @@ -395,10 +378,7 @@ class VISP_EXPORT vpMbTracker \param cMo_ : the pose */ - virtual inline void getPose(vpHomogeneousMatrix &cMo_) const - { - cMo_ = this->cMo; - } + virtual inline void getPose(vpHomogeneousMatrix &cMo_) const { cMo_ = this->cMo; } /*! Get the current pose between the object and the camera. @@ -409,38 +389,26 @@ class VISP_EXPORT vpMbTracker */ virtual inline vpHomogeneousMatrix getPose() const { return this->cMo; } - virtual inline double getStopCriteriaEpsilon() const - { - return m_stopCriteriaEpsilon; - } + virtual inline double getStopCriteriaEpsilon() const { return m_stopCriteriaEpsilon; } // Intializer #ifdef VISP_HAVE_MODULE_GUI - virtual void initClick(const vpImage &I, - const std::string &initFile, - const bool displayHelp = false); - virtual void initClick(const vpImage &I, - const std::vector &points3D_list, + virtual void initClick(const vpImage &I, const std::string &initFile, const bool displayHelp = false); + virtual void initClick(const vpImage &I, const std::vector &points3D_list, const std::string &displayFile = ""); #endif - virtual void initFromPoints(const vpImage &I, - const std::string &initFile); - virtual void initFromPoints(const vpImage &I, - const std::vector &points2D_list, + virtual void initFromPoints(const vpImage &I, const std::string &initFile); + virtual void initFromPoints(const vpImage &I, const std::vector &points2D_list, const std::vector &points3D_list); - virtual void initFromPose(const vpImage &I, - const std::string &initFile); - virtual void initFromPose(const vpImage &I, - const vpHomogeneousMatrix &cMo); - virtual void initFromPose(const vpImage &I, - const vpPoseVector &cPo); + virtual void initFromPose(const vpImage &I, const std::string &initFile); + virtual void initFromPose(const vpImage &I, const vpHomogeneousMatrix &cMo); + virtual void initFromPose(const vpImage &I, const vpPoseVector &cPo); virtual void loadModel(const char *modelFile, const bool verbose = false); - virtual void loadModel(const std::string &modelFile, - const bool verbose = false); + virtual void loadModel(const std::string &modelFile, const bool verbose = false); /*! Set the angle used to test polygons appearance. @@ -462,20 +430,14 @@ class VISP_EXPORT vpMbTracker \param a : new angle in radian. */ - virtual inline void setAngleDisappear(const double &a) - { - angleDisappears = a; - } + 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 setCameraParameters(const vpCameraParameters &camera) { this->cam = camera; } virtual void setClipping(const unsigned int &flags); @@ -487,10 +449,7 @@ class VISP_EXPORT vpMbTracker \sa getCovarianceMatrix() */ - virtual void setCovarianceComputation(const bool &flag) - { - computeCovariance = flag; - } + virtual void setCovarianceComputation(const bool &flag) { computeCovariance = flag; } /*! Enable to display the features. By features, we meant the moving edges @@ -508,10 +467,7 @@ class VISP_EXPORT vpMbTracker \param displayF : set it to true to display the features. */ - virtual void setDisplayFeatures(const bool displayF) - { - displayFeatures = displayF; - } + virtual void setDisplayFeatures(const bool displayF) { displayFeatures = displayF; } virtual void setEstimatedDoF(const vpColVector &v); @@ -540,11 +496,9 @@ class VISP_EXPORT vpMbTracker */ virtual inline void setMaxIter(const unsigned int max) { m_maxIter = max; } - virtual void setMinLineLengthThresh(const double minLineLengthThresh, - 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 setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name = ""); virtual void setNearClippingDistance(const double &dist); @@ -553,11 +507,7 @@ class VISP_EXPORT vpMbTracker \param opt : Optimization method to use. */ - virtual inline void - setOptimizationMethod(const vpMbtOptimizationMethod &opt) - { - m_optimizationMethod = opt; - } + virtual inline void setOptimizationMethod(const vpMbtOptimizationMethod &opt) { m_optimizationMethod = opt; } /*! Set the minimal error (previous / current estimation) to determine if @@ -565,10 +515,7 @@ class VISP_EXPORT vpMbTracker \param eps : Epsilon threshold. */ - virtual inline void setStopCriteriaEpsilon(const double eps) - { - m_stopCriteriaEpsilon = eps; - } + virtual inline void setStopCriteriaEpsilon(const double eps) { m_stopCriteriaEpsilon = eps; } /*! Set if the projection error criteria has to be computed. This criteria @@ -581,10 +528,7 @@ class VISP_EXPORT vpMbTracker \sa getProjectionError() */ - virtual void setProjectionErrorComputation(const bool &flag) - { - computeProjError = flag; - } + virtual void setProjectionErrorComputation(const bool &flag) { computeProjError = flag; } virtual void setScanLineVisibilityTest(const bool &v) { useScanLine = v; } @@ -602,10 +546,7 @@ class VISP_EXPORT vpMbTracker \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%). */ - virtual void setGoodNbRayCastingAttemptsRatio(const double &ratio) - { - faces.setGoodNbRayCastingAttemptsRatio(ratio); - } + virtual void setGoodNbRayCastingAttemptsRatio(const double &ratio) { faces.setGoodNbRayCastingAttemptsRatio(ratio); } /*! 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 @@ -615,8 +556,7 @@ class VISP_EXPORT vpMbTracker \param attempts Number of rays to be sent. */ - virtual void - setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) + virtual void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { faces.setNbRayCastingAttemptsForVisibility(attempts); } @@ -632,10 +572,7 @@ class VISP_EXPORT vpMbTracker Ogre rendering options) when Ogre visibility is enabled. By default, this functionality is turned off. */ - inline virtual void setOgreShowConfigDialog(const bool showConfigDialog) - { - ogreShowConfigDialog = showConfigDialog; - } + inline virtual void setOgreShowConfigDialog(const bool showConfigDialog) { ogreShowConfigDialog = showConfigDialog; } /*! Set the filename used to save the initial pose computed using the @@ -647,10 +584,7 @@ class VISP_EXPORT vpMbTracker \param filename : The new filename. */ - inline void setPoseSavingFilename(const std::string &filename) - { - poseSavingFilename = filename; - } + inline void setPoseSavingFilename(const std::string &filename) { poseSavingFilename = filename; } /* PURE VIRTUAL METHODS */ @@ -666,11 +600,8 @@ class VISP_EXPORT vpMbTracker \param displayFullModel : If true, the full model is displayed (even the non visible surfaces). */ - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false) = 0; + virtual void display(const vpImage &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. @@ -683,11 +614,8 @@ class VISP_EXPORT vpMbTracker \param displayFullModel : If true, the full model is displayed (even the non visible surfaces). */ - virtual void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false) = 0; + virtual void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false) = 0; /*! Initialise the tracking. @@ -720,8 +648,7 @@ class VISP_EXPORT vpMbTracker \param I : image corresponding to the desired pose. \param cdMo : Pose to affect. */ - virtual void setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo) = 0; + virtual void setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo) = 0; /*! Test the quality of the tracking. @@ -741,64 +668,45 @@ class VISP_EXPORT vpMbTracker protected: /** @name Protected Member Functions Inherited from vpMbTracker */ //@{ - void addPolygon(const std::vector &corners, const int idFace = -1, - const std::string &polygonName = "", - const bool useLod = false, - const double minPolygonAreaThreshold = 2500.0, + void addPolygon(const std::vector &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, + 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 > &listFaces, - const int idFace = -1, const std::string &polygonName = "", - const bool useLod = false, + 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 > &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, + void createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius, std::vector > &listFaces); - virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, - const vpColVector &w_true, - const vpHomogeneousMatrix &cMoPrev, - const vpMatrix &L_true, - const vpMatrix &LVJ_true, - const vpColVector &error); - - void computeJTR(const vpMatrix &J, const vpColVector &R, - vpColVector &JTR) const; - - virtual void computeVVSCheckLevenbergMarquardt( - const unsigned int iter, vpColVector &error, - const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, - double &mu, bool &reStartFromLastIncrement, vpColVector *const w = NULL, - const vpColVector *const m_w_prev = NULL); + virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, + const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, + const vpMatrix &LVJ_true, const vpColVector &error); + + void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const; + + virtual void computeVVSCheckLevenbergMarquardt(const unsigned int iter, vpColVector &error, + const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, + double &mu, bool &reStartFromLastIncrement, + vpColVector *const w = NULL, const vpColVector *const m_w_prev = NULL); virtual void computeVVSInit() = 0; virtual void computeVVSInteractionMatrixAndResidu() = 0; - virtual void computeVVSPoseEstimation( - const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L, - vpMatrix <L, vpColVector &R, const vpColVector &error, - vpColVector &error_prev, vpColVector <R, double &mu, vpColVector &v, - const vpColVector *const w = NULL, vpColVector *const m_w_prev = NULL); - virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, - vpColVector &w); + virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L, vpMatrix <L, + vpColVector &R, const vpColVector &error, vpColVector &error_prev, + vpColVector <R, double &mu, vpColVector &v, const vpColVector *const w = NULL, + vpColVector *const m_w_prev = NULL); + virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w); #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, + 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 extractCylinders(SoVRMLIndexedFaceSet *face_set, - vpHomogeneousMatrix &transform, int &idFace, + 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 @@ -816,10 +724,8 @@ class VISP_EXPORT vpMbTracker \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; + 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. @@ -830,8 +736,7 @@ class VISP_EXPORT vpMbTracker \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, + virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace = 0, const std::string &name = "") = 0; /*! @@ -850,10 +755,8 @@ class VISP_EXPORT vpMbTracker virtual void initFaceFromLines(vpMbtPolygon &polygon) = 0; virtual void loadVRMLModel(const std::string &modelFile); - virtual void loadCAOModel(const std::string &modelFile, - std::vector &vectorOfModelFilename, - int &startIdFace, const bool verbose = false, - const bool parent = true); + virtual void loadCAOModel(const std::string &modelFile, std::vector &vectorOfModelFilename, + int &startIdFace, const bool verbose = false, const bool parent = true); void removeComment(std::ifstream &fileId); @@ -872,18 +775,13 @@ class VISP_EXPORT vpMbTracker inline std::string <rim(std::string &s) const { - s.erase(s.begin(), - std::find_if(s.begin(), s.end(), - std::not1(std::ptr_fun(std::isspace)))); + s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); return s; } inline std::string &rtrim(std::string &s) const { - s.erase(std::find_if(s.rbegin(), s.rend(), - std::not1(std::ptr_fun(std::isspace))) - .base(), - s.end()); + s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); return s; } diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbXmlParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbXmlParser.h index e32b62974c..81c5538eda 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbXmlParser.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbXmlParser.h @@ -130,10 +130,7 @@ class VISP_EXPORT vpMbXmlParser : public vpXmlParser */ inline double getAngleDisappear() const { return angleDisappear; } - void getCameraParameters(vpCameraParameters &_cam) const - { - _cam = this->cam; - } + void getCameraParameters(vpCameraParameters &_cam) const { _cam = this->cam; } /*! Get the far clipping distance. @@ -161,20 +158,14 @@ class VISP_EXPORT vpMbXmlParser : public vpXmlParser \return The minimum line length. */ - inline double getMinLineLengthThreshold() const - { - return minLineLengthThreshold; - } + 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; - } + inline double getMinPolygonAreaThreshold() const { return minPolygonAreaThreshold; } /*! Get the near clipping distance. @@ -216,10 +207,7 @@ class VISP_EXPORT vpMbXmlParser : public vpXmlParser \param adisappear : New angleDisappear */ - inline void setAngleDisappear(const double &adisappear) - { - angleDisappear = adisappear; - } + inline void setAngleDisappear(const double &adisappear) { angleDisappear = adisappear; } void setCameraParameters(const vpCameraParameters &_cam) { cam = _cam; } @@ -228,20 +216,14 @@ class VISP_EXPORT vpMbXmlParser : public vpXmlParser \param fclip : New farClipping */ - inline void setFarClippingDistance(const double &fclip) - { - farClipping = fclip; - } + 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; - } + inline void setNearClippingDistance(const double &nclip) { nearClipping = nclip; } void writeMainClass(xmlNodePtr node); //@} diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCircle.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCircle.h index ae740c04fe..72789de21b 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCircle.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCircle.h @@ -126,19 +126,14 @@ class VISP_EXPORT vpMbtDistanceCircle vpMbtDistanceCircle(); ~vpMbtDistanceCircle(); - void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, - const double r); + void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r); void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo); - void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - void display(const vpImage &I, const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); void displayMovingEdges(const vpImage &I); /*! @@ -146,10 +141,7 @@ class VISP_EXPORT vpMbtDistanceCircle \param camera : The vpCameraParameters used to store the camera parameters. */ - inline void getCameraParameters(vpCameraParameters &camera) - { - camera = this->cam; - } + inline void getCameraParameters(vpCameraParameters &camera) { camera = this->cam; } /*! Get the index of the circle. @@ -183,8 +175,7 @@ class VISP_EXPORT vpMbtDistanceCircle void initInteractionMatrixError(); - bool initMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo); + bool initMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo); /*! Return if the circle is used for tracking. @@ -199,17 +190,13 @@ class VISP_EXPORT vpMbtDistanceCircle */ inline bool isVisible() const { return isvisible; } - void reinitMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo); + void reinitMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo); /*! Set the camera paramters. \param camera : The camera parameters. */ - inline void setCameraParameters(const vpCameraParameters &camera) - { - this->cam = camera; - } + inline void setCameraParameters(const vpCameraParameters &camera) { this->cam = camera; } /*! Set if the circle has to considered during tracking phase. @@ -239,20 +226,14 @@ class VISP_EXPORT vpMbtDistanceCircle \param circle_name : The name of the circle. */ - inline void setName(const std::string &circle_name) - { - this->name = circle_name; - } + 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); - } + 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 @@ -262,11 +243,9 @@ class VISP_EXPORT vpMbtDistanceCircle */ inline void setVisible(bool _isvisible) { isvisible = _isvisible; } - void trackMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo); + void trackMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo); - void updateMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo); + void updateMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo); private: void project(const vpHomogeneousMatrix &cMo); diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCylinder.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCylinder.h index 2085bde304..6e03f54358 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCylinder.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCylinder.h @@ -142,17 +142,12 @@ class VISP_EXPORT vpMbtDistanceCylinder void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r); - void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, - const vpImage &I); - - void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - void display(const vpImage &I, const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage &I); + + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); void displayMovingEdges(const vpImage &I); /*! @@ -160,10 +155,7 @@ class VISP_EXPORT vpMbtDistanceCylinder \param camera : The vpCameraParameters used to store the camera parameters. */ - inline void getCameraParameters(vpCameraParameters &camera) - { - camera = this->cam; - } + inline void getCameraParameters(vpCameraParameters &camera) { camera = this->cam; } /*! Get the index of the cylinder. @@ -199,8 +191,7 @@ class VISP_EXPORT vpMbtDistanceCylinder void initInteractionMatrixError(); - bool initMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo); + bool initMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo); /*! Return if the cylinder is used for tracking. @@ -216,27 +207,20 @@ class VISP_EXPORT vpMbtDistanceCylinder */ inline bool isVisible() const { return isvisible; } - void reinitMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo); + void reinitMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo); /*! Set the camera paramters. \param camera : The camera parameters. */ - inline void setCameraParameters(const vpCameraParameters &camera) - { - this->cam = camera; - } + 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; - } + inline void setTracked(const bool &track) { this->isTrackedCylinder = track; } /*! Set the index of the cylinder. @@ -273,10 +257,7 @@ class VISP_EXPORT vpMbtDistanceCylinder \param cyl_name : The name of the cylinder */ - inline void setName(const char *cyl_name) - { - this->name = std::string(cyl_name); - } + 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 @@ -286,11 +267,9 @@ class VISP_EXPORT vpMbtDistanceCylinder */ inline void setVisible(bool _isvisible) { isvisible = _isvisible; } - void trackMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo); + void trackMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo); - void updateMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo); + void updateMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo); private: void project(const vpHomogeneousMatrix &cMo); diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltCylinder.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltCylinder.h index 4a963da177..846f81e8ea 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltCylinder.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltCylinder.h @@ -41,8 +41,7 @@ #include -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) #include @@ -139,17 +138,12 @@ class VISP_EXPORT 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 &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - void display(const vpImage &I, const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J); + + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); void displayPrimitive(const vpImage &_I); void displayPrimitive(const vpImage &_I); @@ -199,35 +193,26 @@ class VISP_EXPORT vpMbtDistanceKltCylinder void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo); - void removeOutliers(const vpColVector &weight, - const double &threshold_outlier); + 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; - } + 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; - } + 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); + 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); + void updateMask(IplImage *mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0); #endif }; diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltPoints.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltPoints.h index 932d4a53c5..2de6cfb3e1 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltPoints.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltPoints.h @@ -42,8 +42,7 @@ #include -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) #include @@ -112,8 +111,7 @@ class VISP_EXPORT vpMbtDistanceKltPoints 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); + 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: @@ -140,18 +138,13 @@ class VISP_EXPORT vpMbtDistanceKltPoints virtual ~vpMbtDistanceKltPoints(); unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker); - void computeHomography(const vpHomogeneousMatrix &_cTc0, - vpHomography &cHc0); + void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0); void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J); - void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - void display(const vpImage &I, const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); void displayPrimitive(const vpImage &_I); void displayPrimitive(const vpImage &_I); @@ -201,35 +194,26 @@ class VISP_EXPORT vpMbtDistanceKltPoints */ inline bool isTracked() const { return isTrackedKltPoints; } - void removeOutliers(const vpColVector &weight, - const double &threshold_outlier); + 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; - } + 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; - } + 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); + 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); + void updateMask(IplImage *mask, unsigned char _nb = 255, unsigned int _shiftBorder = 0); #endif }; diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceLine.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceLine.h index f7fafdf326..0bbe5fa78f 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceLine.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceLine.h @@ -137,18 +137,13 @@ class VISP_EXPORT vpMbtDistanceLine void buildFrom(vpPoint &_p1, vpPoint &_p2); - bool closeToImageBorder(const vpImage &I, - const unsigned int threshold); + bool closeToImageBorder(const vpImage &I, const unsigned int threshold); void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo); - void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - void display(const vpImage &I, const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); void displayMovingEdges(const vpImage &I); /*! @@ -156,10 +151,7 @@ class VISP_EXPORT vpMbtDistanceLine \param camera : The vpCameraParameters used to store the camera parameters. */ - inline void getCameraParameters(vpCameraParameters &camera) const - { - camera = this->cam; - } + inline void getCameraParameters(vpCameraParameters &camera) const { camera = this->cam; } /*! Get the index of the line. @@ -193,8 +185,7 @@ class VISP_EXPORT vpMbtDistanceLine void initInteractionMatrixError(); - bool initMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo); + bool initMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo); /*! Return if the line is used for tracking. @@ -210,17 +201,13 @@ class VISP_EXPORT vpMbtDistanceLine */ inline bool isVisible() const { return isvisible; } - void reinitMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo); + void reinitMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo); /*! Set the camera paramters. \param camera : The camera parameters. */ - inline void setCameraParameters(const vpCameraParameters &camera) - { - this->cam = camera; - } + inline void setCameraParameters(const vpCameraParameters &camera) { this->cam = camera; } /*! Set the index of the line. @@ -243,20 +230,14 @@ class VISP_EXPORT vpMbtDistanceLine \param line_name : The name of the line. */ - inline void setName(const std::string &line_name) - { - this->name = line_name; - } + 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); - } + inline void setName(const char *line_name) { this->name = std::string(line_name); } void setTracked(const std::string &name, const bool &track); @@ -268,11 +249,9 @@ class VISP_EXPORT vpMbtDistanceLine */ void setVisible(bool _isvisible) { isvisible = _isvisible; } - void trackMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo); + void trackMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo); - void updateMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo); + void updateMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo); void updateTracked(); diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h index f33ed56371..0cb5bb5e46 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h @@ -63,8 +63,7 @@ Data parser for the model based tracker. */ -class VISP_EXPORT vpMbtEdgeKltXmlParser : public vpMbtXmlParser, - public vpMbtKltXmlParser +class VISP_EXPORT vpMbtEdgeKltXmlParser : public vpMbtXmlParser, public vpMbtKltXmlParser { protected: typedef enum { camera, face, klt, ecm, lod } dataToParseMbtEdgeKlt; diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthDense.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthDense.h index c02d4c81b0..9040daf2bc 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthDense.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthDense.h @@ -58,10 +58,10 @@ class VISP_EXPORT vpMbtFaceDepthDense DEPTH_OCCUPANCY_RATIO_FILTERING = 1 << 1, ///< Face is used if there is ///< enough depth information in ///< the face polygon - MIN_DISTANCE_FILTERING = 1 << 2, ///< Face is used if the camera position - ///< is farther than the threshold - MAX_DISTANCE_FILTERING = 1 << 3 ///< Face is used if the camera position - ///< is closer than the threshold + MIN_DISTANCE_FILTERING = 1 << 2, ///< Face is used if the camera position + ///< is farther than the threshold + MAX_DISTANCE_FILTERING = 1 << 3 ///< Face is used if the camera position + ///< is closer than the threshold }; //! Camera intrinsic parameters @@ -84,64 +84,44 @@ class VISP_EXPORT vpMbtFaceDepthDense vpMbtFaceDepthDense(); virtual ~vpMbtFaceDepthDense(); - void addLine(vpPoint &p1, vpPoint &p2, - vpMbHiddenFaces *const faces, int polygon = -1, + void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces *const faces, int polygon = -1, std::string name = ""); #ifdef VISP_HAVE_PCL - bool computeDesiredFeatures( - const vpHomogeneousMatrix &cMo, - const pcl::PointCloud::ConstPtr &point_cloud, - const unsigned int stepX, const unsigned int stepY + bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, + const pcl::PointCloud::ConstPtr &point_cloud, const unsigned int stepX, + const unsigned int stepY #if DEBUG_DISPLAY_DEPTH_DENSE - , - vpImage &debugImage, - std::vector > &roiPts_vec + , + vpImage &debugImage, std::vector > &roiPts_vec #endif ); #endif - bool - computeDesiredFeatures(const vpHomogeneousMatrix &cMo, - const unsigned int width, const unsigned int height, - const std::vector &point_cloud, - const unsigned int stepX, const unsigned int stepY + bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height, + const std::vector &point_cloud, const unsigned int stepX, + const unsigned int stepY #if DEBUG_DISPLAY_DEPTH_DENSE - , - vpImage &debugImage, - std::vector > &roiPts_vec + , + vpImage &debugImage, std::vector > &roiPts_vec #endif ); - void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, - vpMatrix &L, vpColVector &error); + void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error); void computeVisibility(); void computeVisibilityDisplay(); - void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - void display(const vpImage &I, const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); - - void displayFeature(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const double scale = 0.05, - const unsigned int thickness = 1); - void displayFeature(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const double scale = 0.05, - const unsigned int thickness = 1); - - inline unsigned int getNbFeatures() const - { - return (unsigned int)(m_pointCloudFace.size() / 3); - } + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + + void displayFeature(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const double scale = 0.05, const unsigned int thickness = 1); + void displayFeature(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const double scale = 0.05, const unsigned int thickness = 1); + + inline unsigned int getNbFeatures() const { return (unsigned int)(m_pointCloudFace.size() / 3); } inline bool isVisible() const { return m_polygon->isvisible; } @@ -154,22 +134,17 @@ class VISP_EXPORT vpMbtFaceDepthDense m_depthDenseFilteringMaxDist = maxDistance; } - inline void setDepthDenseFilteringMethod(const int method) - { - m_depthDenseFilteringMethod = method; - } + inline void setDepthDenseFilteringMethod(const int method) { m_depthDenseFilteringMethod = method; } inline void setDepthDenseFilteringMinDistance(const double minDistance) { m_depthDenseFilteringMinDist = minDistance; } - inline void - setDepthDenseFilteringOccupancyRatio(const double occupancyRatio) + inline void setDepthDenseFilteringOccupancyRatio(const double occupancyRatio) { if (occupancyRatio < 0.0 || occupancyRatio > 1.0) { - std::cerr << "occupancyRatio < 0.0 || occupancyRatio > 1.0" - << std::endl; + std::cerr << "occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl; } else { m_depthDenseFilteringOccupancyRatio = occupancyRatio; } @@ -193,8 +168,7 @@ class VISP_EXPORT vpMbtFaceDepthDense PolygonLine() : m_p1(NULL), m_p2(NULL), m_poly(), m_imPt1(), m_imPt2() {} PolygonLine(const PolygonLine &polyLine) - : m_p1(NULL), m_p2(NULL), m_poly(polyLine.m_poly), - m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2) + : m_p1(NULL), m_p2(NULL), m_poly(polyLine.m_poly), m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2) { m_p1 = &m_poly.p[0]; m_p2 = &m_poly.p[1]; @@ -241,8 +215,8 @@ class VISP_EXPORT vpMbtFaceDepthDense std::vector m_polygonLines; protected: - void computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width, - const unsigned int height, std::vector &roiPts + void computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height, + std::vector &roiPts #if DEBUG_DISPLAY_DEPTH_DENSE , std::vector > &roiPts_vec diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h index 73923118be..9649883e81 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h @@ -86,103 +86,63 @@ class VISP_EXPORT vpMbtFaceDepthNormal vpMbtFaceDepthNormal(); virtual ~vpMbtFaceDepthNormal(); - void addLine(vpPoint &p1, vpPoint &p2, - vpMbHiddenFaces *const faces, int polygon = -1, + void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces *const faces, int polygon = -1, std::string name = ""); #ifdef VISP_HAVE_PCL - bool computeDesiredFeatures( - const vpHomogeneousMatrix &cMo, const unsigned int width, - const unsigned int height, - const pcl::PointCloud::ConstPtr &point_cloud, - vpColVector &desired_features, const unsigned int stepX, - const unsigned int stepY + bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height, + const pcl::PointCloud::ConstPtr &point_cloud, + vpColVector &desired_features, const unsigned int stepX, const unsigned int stepY #if DEBUG_DISPLAY_DEPTH_NORMAL - , - vpImage &debugImage, - std::vector > &roiPts_vec + , + vpImage &debugImage, std::vector > &roiPts_vec #endif ); #endif - bool - computeDesiredFeatures(const vpHomogeneousMatrix &cMo, - const unsigned int width, const unsigned int height, - const std::vector &point_cloud, - vpColVector &desired_features, - const unsigned int stepX, const unsigned int stepY + bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height, + const std::vector &point_cloud, vpColVector &desired_features, + const unsigned int stepX, const unsigned int stepY #if DEBUG_DISPLAY_DEPTH_NORMAL - , - vpImage &debugImage, - std::vector > &roiPts_vec + , + vpImage &debugImage, std::vector > &roiPts_vec #endif ); - void computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, - vpColVector &features); + void computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features); void computeVisibility(); void computeVisibilityDisplay(); - void computeNormalVisibility(const double nx, const double ny, - const double nz, - const vpColVector ¢roid_point, + void computeNormalVisibility(const double nx, const double ny, const double nz, const vpColVector ¢roid_point, vpColVector &face_normal); #ifdef VISP_HAVE_PCL - void computeNormalVisibility(const float nx, const float ny, const float nz, - const pcl::PointXYZ ¢roid_point, + void computeNormalVisibility(const float nx, const float ny, const float nz, const pcl::PointXYZ ¢roid_point, pcl::PointXYZ &face_normal); #endif - void computeNormalVisibility(const double nx, const double ny, - const double nz, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &camera, - vpColVector &correct_normal, - vpPoint ¢roid); - - void display(const vpImage &I, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - const vpColor &col, const unsigned int thickness = 1, - const bool displayFullModel = false); - void display(const vpImage &I, const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, const vpColor &col, - const unsigned int thickness = 1, - const bool displayFullModel = false); - - void displayFeature(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const double scale = 0.05, - const unsigned int thickness = 1); - void displayFeature(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const double scale = 0.05, - const unsigned int thickness = 1); + void computeNormalVisibility(const double nx, const double ny, const double nz, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &camera, vpColVector &correct_normal, vpPoint ¢roid); + + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + void display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false); + + void displayFeature(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const double scale = 0.05, const unsigned int thickness = 1); + void displayFeature(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + const double scale = 0.05, const unsigned int thickness = 1); inline bool isVisible() const { return m_polygon->isvisible; } void setCameraParameters(const vpCameraParameters &camera); - inline void setFaceCentroidMethod(const vpFaceCentroidType &method) - { - m_faceCentroidMethod = method; - } + inline void setFaceCentroidMethod(const vpFaceCentroidType &method) { m_faceCentroidMethod = method; } - inline void - setFeatureEstimationMethod(const vpFeatureEstimationType &method) - { - m_featureEstimationMethod = method; - } + inline void setFeatureEstimationMethod(const vpFeatureEstimationType &method) { m_featureEstimationMethod = method; } - inline void setPclPlaneEstimationMethod(const int method) - { - m_pclPlaneEstimationMethod = method; - } + inline void setPclPlaneEstimationMethod(const int method) { m_pclPlaneEstimationMethod = method; } - inline void setPclPlaneEstimationRansacMaxIter(const int maxIter) - { - m_pclPlaneEstimationRansacMaxIter = maxIter; - } + inline void setPclPlaneEstimationRansacMaxIter(const int maxIter) { m_pclPlaneEstimationRansacMaxIter = maxIter; } inline void setPclPlaneEstimationRansacThreshold(const double threshold) { @@ -209,8 +169,7 @@ class VISP_EXPORT vpMbtFaceDepthNormal PolygonLine() : m_p1(NULL), m_p2(NULL), m_poly(), m_imPt1(), m_imPt2() {} PolygonLine(const PolygonLine &polyLine) - : m_p1(NULL), m_p2(NULL), m_poly(polyLine.m_poly), - m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2) + : m_p1(NULL), m_p2(NULL), m_poly(polyLine.m_poly), m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2) { m_p1 = &m_poly.p[0]; m_p2 = &m_poly.p[1]; @@ -248,8 +207,7 @@ class VISP_EXPORT vpMbtFaceDepthNormal Mat33 inverse() const { // determinant - T det = data[0] * (data[4] * data[8] - data[7] * data[5]) - - data[1] * (data[3] * data[8] - data[5] * data[6]) + + T det = data[0] * (data[4] * data[8] - data[7] * data[5]) - data[1] * (data[3] * data[8] - data[5] * data[6]) + data[2] * (data[3] * data[7] - data[4] * data[6]); T invdet = 1 / det; @@ -298,45 +256,35 @@ class VISP_EXPORT vpMbtFaceDepthNormal std::vector m_polygonLines; #ifdef VISP_HAVE_PCL - bool computeDesiredFeaturesPCL( - const pcl::PointCloud::ConstPtr &point_cloud_face, - vpColVector &desired_features, vpColVector &desired_normal, - vpColVector ¢roid_point); + bool computeDesiredFeaturesPCL(const pcl::PointCloud::ConstPtr &point_cloud_face, + vpColVector &desired_features, vpColVector &desired_normal, + vpColVector ¢roid_point); #endif - void computeDesiredFeaturesRobustFeatures( - const std::vector &point_cloud_face_custom, - const std::vector &point_cloud_face, - const vpHomogeneousMatrix &cMo, vpColVector &desired_features, - vpColVector &desired_normal, vpColVector ¢roid_point); - void computeDesiredFeaturesSVD(const std::vector &point_cloud_face, - const vpHomogeneousMatrix &cMo, - vpColVector &desired_features, - vpColVector &desired_normal, + void computeDesiredFeaturesRobustFeatures(const std::vector &point_cloud_face_custom, + const std::vector &point_cloud_face, const vpHomogeneousMatrix &cMo, + vpColVector &desired_features, vpColVector &desired_normal, + vpColVector ¢roid_point); + void computeDesiredFeaturesSVD(const std::vector &point_cloud_face, const vpHomogeneousMatrix &cMo, + vpColVector &desired_features, vpColVector &desired_normal, vpColVector ¢roid_point); - void computeDesiredNormalAndCentroid(const vpHomogeneousMatrix &cMo, - const vpColVector &desired_normal, + void computeDesiredNormalAndCentroid(const vpHomogeneousMatrix &cMo, const vpColVector &desired_normal, const vpColVector ¢roid_point); - bool computePolygonCentroid(const std::vector &points, - vpPoint ¢roid); + bool computePolygonCentroid(const std::vector &points, vpPoint ¢roid); - void computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width, - const unsigned int height, std::vector &roiPts + void computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height, + std::vector &roiPts #if DEBUG_DISPLAY_DEPTH_NORMAL , std::vector > &roiPts_vec #endif ); - void estimateFeatures(const std::vector &point_cloud_face, - const vpHomogeneousMatrix &cMo, - vpColVector &x_estimated, - std::vector &weights); + void estimateFeatures(const std::vector &point_cloud_face, const vpHomogeneousMatrix &cMo, + vpColVector &x_estimated, std::vector &weights); - void estimatePlaneEquationSVD(const std::vector &point_cloud_face, - const vpHomogeneousMatrix &cMo, - vpColVector &plane_equation_estimated, - vpColVector ¢roid); + void estimatePlaneEquationSVD(const std::vector &point_cloud_face, const vpHomogeneousMatrix &cMo, + vpColVector &plane_equation_estimated, vpColVector ¢roid); bool samePoint(const vpPoint &P1, const vpPoint &P2) const; }; diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h index 5b0626bab3..661672709a 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h @@ -104,8 +104,6 @@ \include tutorial-me-ellipse-tracker.cpp - */ -/* The code below shows how to use this class. \code #include @@ -130,8 +128,8 @@ int main() vpMbtMeEllipse ellipse; ellipse.setMe(&me); - // Initialize the tracking. You have to click on five different points -belonging to the ellipse. ellipse.initTracking(I); + // Initialize the tracking. You have to click on five different points belonging to the ellipse. + ellipse.initTracking(I); while ( 1 ) { @@ -155,9 +153,7 @@ class VISP_EXPORT vpMbtMeEllipse : public vpMeTracker vpMbtMeEllipse(const vpMbtMeEllipse &meellipse); virtual ~vpMbtMeEllipse(); - void computeProjectionError(const vpImage &_I, - double &_sumErrorRad, - unsigned int &_nbFeatures); + void computeProjectionError(const vpImage &_I, double &_sumErrorRad, unsigned int &_nbFeatures); void display(const vpImage &I, vpColor col); void display(const vpImage &I) @@ -221,13 +217,12 @@ class VISP_EXPORT vpMbtMeEllipse : public vpMeTracker E = e; } - void initTracking(const vpImage &I, const vpImagePoint &ic, - double mu20_p, double mu11_p, double mu02_p); + void initTracking(const vpImage &I, const vpImagePoint &ic, double mu20_p, double mu11_p, + double mu02_p); void track(const vpImage &Im); - void updateParameters(const vpImage &I, - const vpImagePoint &ic, double mu20_p, double mu11_p, + void updateParameters(const vpImage &I, const vpImagePoint &ic, double mu20_p, double mu11_p, double mu02_p); protected: diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtMeLine.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtMeLine.h index e5640eb80f..83e5340ff5 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtMeLine.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtMeLine.h @@ -76,9 +76,7 @@ class VISP_EXPORT vpMbtMeLine : public vpMeTracker vpMbtMeLine(); ~vpMbtMeLine(); - void computeProjectionError(const vpImage &_I, - double &_sumErrorRad, - unsigned int &_nbFeatures); + void computeProjectionError(const vpImage &_I, double &_sumErrorRad, unsigned int &_nbFeatures); void display(const vpImage & /*I*/, vpColor /*col*/) { ; } void display(const vpImage &I) @@ -110,16 +108,14 @@ class VISP_EXPORT vpMbtMeLine : public vpMeTracker */ inline double get_c() const { return this->c; } - void initTracking(const vpImage &I, const vpImagePoint &ip1, - const vpImagePoint &ip2, double rho, double theta); + void initTracking(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, double rho, + double theta); void track(const vpImage &I); - void updateParameters(const vpImage &I, double rho, + void updateParameters(const vpImage &I, double rho, double theta); + void updateParameters(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, double rho, double theta); - void updateParameters(const vpImage &I, - const vpImagePoint &ip1, const vpImagePoint &ip2, - double rho, double theta); private: void bubbleSortI(); @@ -129,8 +125,7 @@ class VISP_EXPORT vpMbtMeLine : public vpMeTracker void setExtremities(); void suppressPoints(const vpImage &I); void reSample(const vpImage &image); - void reSample(const vpImage &image, const vpImagePoint &ip1, - const vpImagePoint &ip2); + void reSample(const vpImage &image, const vpImagePoint &ip1, const vpImagePoint &ip2); void updateDelta(); }; diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtPolygon.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtPolygon.h index 2812d15675..3a5ca3f867 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtPolygon.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtPolygon.h @@ -109,11 +109,9 @@ class VISP_EXPORT vpMbtPolygon : public vpPolygon3D 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 &I = vpImage()); + virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo = false, + const vpCameraParameters &cam = vpCameraParameters(), + const vpImage &I = vpImage()); bool isVisible() const { return isvisible; } vpMbtPolygon &operator=(const vpMbtPolygon &mbtp); @@ -140,10 +138,7 @@ class VISP_EXPORT vpMbtPolygon : public vpPolygon3D \sa setLoD() */ - inline void setMinLineLengthThresh(const double min_line_length) - { - this->minLineLengthThresh = min_line_length; - } + 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. @@ -154,30 +149,21 @@ class VISP_EXPORT vpMbtPolygon : public vpPolygon3D \sa setLoD() */ - inline void setMinPolygonAreaThresh(const double min_polygon_area) - { - this->minPolygonAreaThresh = min_polygon_area; - } + 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; - } + 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; - } + inline void setIsPolygonOriented(const bool &oriented) { this->hasOrientation = oriented; } }; #endif diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h index a551855ae9..c81f495201 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h @@ -44,17 +44,13 @@ template class VISP_EXPORT vpMbtTukeyEstimator { public: - void MEstimator(const std::vector &residues, std::vector &weights, - const T NoiseThreshold); - void MEstimator(const vpColVector &residues, vpColVector &weights, - const double NoiseThreshold); + void MEstimator(const std::vector &residues, std::vector &weights, const T NoiseThreshold); + void MEstimator(const vpColVector &residues, vpColVector &weights, const double NoiseThreshold); private: T getMedian(std::vector &vec); - void MEstimator_impl(const std::vector &residues, - std::vector &weights, const T NoiseThreshold); - void MEstimator_impl_ssse3(const std::vector &residues, - std::vector &weights, const T NoiseThreshold); + void MEstimator_impl(const std::vector &residues, std::vector &weights, const T NoiseThreshold); + void MEstimator_impl_ssse3(const std::vector &residues, std::vector &weights, const T NoiseThreshold); void psiTukey(const T sig, std::vector &x, std::vector &weights); void psiTukey(const T sig, std::vector &x, vpColVector &weights); diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlGenericParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlGenericParser.h index 312a390bfb..a8597ac2d0 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlGenericParser.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlGenericParser.h @@ -65,10 +65,9 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser { public: enum vpParserType { - EDGE_PARSER = 1 << 0, /*!< Parser for model-based tracking using moving - edges features. */ - KLT_PARSER = - 1 << 1, /*!< Parser for model-based tracking using KLT features. */ + EDGE_PARSER = 1 << 0, /*!< Parser for model-based tracking using moving + edges features. */ + KLT_PARSER = 1 << 1, /*!< Parser for model-based tracking using KLT features. */ DEPTH_NORMAL_PARSER = 1 << 2, /*!< Parser for model-based tracking using depth normal features. */ DEPTH_DENSE_PARSER = 1 << 3 /*!< Parser for model-based tracking using @@ -123,8 +122,7 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser unsigned int m_kltPyramidLevels; // Depth normal //! Feature estimation method - vpMbtFaceDepthNormal::vpFeatureEstimationType - m_depthNormalFeatureEstimationMethod; + vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod; //! PCL plane estimation method int m_depthNormalPclPlaneEstimationMethod; //! PCL RANSAC maximum number of iterations @@ -229,24 +227,17 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser /*! Get depth dense sampling step in X. */ - inline unsigned int getDepthDenseSamplingStepX() const - { - return m_depthDenseSamplingStepX; - } + inline unsigned int getDepthDenseSamplingStepX() const { return m_depthDenseSamplingStepX; } /*! Get depth dense sampling step in Y. */ - inline unsigned int getDepthDenseSamplingStepY() const - { - return m_depthDenseSamplingStepY; - } + inline unsigned int getDepthDenseSamplingStepY() const { return m_depthDenseSamplingStepY; } /*! Get depth normal feature estimation method. */ - vpMbtFaceDepthNormal::vpFeatureEstimationType - getDepthNormalFeatureEstimationMethod() const + vpMbtFaceDepthNormal::vpFeatureEstimationType getDepthNormalFeatureEstimationMethod() const { return m_depthNormalFeatureEstimationMethod; } @@ -254,10 +245,7 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser /*! Get depth normal PCL plane estimation method. */ - inline int getDepthNormalPclPlaneEstimationMethod() const - { - return m_depthNormalPclPlaneEstimationMethod; - } + inline int getDepthNormalPclPlaneEstimationMethod() const { return m_depthNormalPclPlaneEstimationMethod; } /*! Get depth normal PCL maximum number of iterations. @@ -278,18 +266,12 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser /*! Get depth normal sampling step in X. */ - inline unsigned int getDepthNormalSamplingStepX() const - { - return m_depthNormalSamplingStepX; - } + inline unsigned int getDepthNormalSamplingStepX() const { return m_depthNormalSamplingStepX; } /*! Get depth normal sampling step in Y. */ - inline unsigned int getDepthNormalSamplingStepY() const - { - return m_depthNormalSamplingStepY; - } + inline unsigned int getDepthNormalSamplingStepY() const { return m_depthNormalSamplingStepY; } /*! Get the far clipping distance. @@ -329,10 +311,7 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser /*! Get the number of pyramid levels */ - inline unsigned int getKltPyramidLevels() const - { - return m_kltPyramidLevels; - } + inline unsigned int getKltPyramidLevels() const { return m_kltPyramidLevels; } /*! Get the quality of the KLT. @@ -352,18 +331,12 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser /*! Get the minimum line length to track a segment when LOD is enabled. */ - inline double getLodMinLineLengthThreshold() const - { - return m_minLineLengthThreshold; - } + inline double getLodMinLineLengthThreshold() const { return m_minLineLengthThreshold; } /*! Get the minimum polygon area to track a face when LOD is enabled. */ - inline double getLodMinPolygonAreaThreshold() const - { - return m_minPolygonAreaThreshold; - } + inline double getLodMinPolygonAreaThreshold() const { return m_minPolygonAreaThreshold; } /*! Get the near clipping distance. @@ -393,58 +366,42 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser \param aappear : New angleAppear */ - inline void setAngleAppear(const double &aappear) - { - m_angleAppear = aappear; - } + inline void setAngleAppear(const double &aappear) { m_angleAppear = aappear; } /*! Set the angle to determine if a face disappeared. \param adisappear : New angleDisappear */ - inline void setAngleDisappear(const double &adisappear) - { - m_angleDisappear = adisappear; - } + inline void setAngleDisappear(const double &adisappear) { m_angleDisappear = adisappear; } /*! Set camera parameters. \param _cam : New camera parameters */ - inline void setCameraParameters(const vpCameraParameters &_cam) - { - m_cam = _cam; - } + inline void setCameraParameters(const vpCameraParameters &_cam) { m_cam = _cam; } /*! Set depth dense sampling step in X. \param stepX : New sampling step */ - inline void setDepthDenseSamplingStepX(const unsigned int stepX) - { - m_depthDenseSamplingStepX = stepX; - } + inline void setDepthDenseSamplingStepX(const unsigned int stepX) { m_depthDenseSamplingStepX = stepX; } /*! Set depth dense sampling step in Y. \param stepY : New sampling step */ - inline void setDepthDenseSamplingStepY(const unsigned int stepY) - { - m_depthDenseSamplingStepY = stepY; - } + inline void setDepthDenseSamplingStepY(const unsigned int stepY) { m_depthDenseSamplingStepY = stepY; } /*! Set depth normal feature estimation method. \param method : New feature estimation method */ - inline void setDepthNormalFeatureEstimationMethod( - const vpMbtFaceDepthNormal::vpFeatureEstimationType &method) + inline void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method) { m_depthNormalFeatureEstimationMethod = method; } @@ -474,8 +431,7 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser \param threshold : New RANSAC threshold */ - inline void - setDepthNormalPclPlaneEstimationRansacThreshold(const double threshold) + inline void setDepthNormalPclPlaneEstimationRansacThreshold(const double threshold) { m_depthNormalPclPlaneEstimationRansacThreshold = threshold; } @@ -485,20 +441,14 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser \param stepX : New sampling step */ - inline void setDepthNormalSamplingStepX(const unsigned int stepX) - { - m_depthNormalSamplingStepX = stepX; - } + inline void setDepthNormalSamplingStepX(const unsigned int stepX) { m_depthNormalSamplingStepX = stepX; } /*! Set depth normal sampling step in Y. \param stepY : New sampling step */ - inline void setDepthNormalSamplingStepY(const unsigned int stepY) - { - m_depthNormalSamplingStepY = stepY; - } + inline void setDepthNormalSamplingStepY(const unsigned int stepY) { m_depthNormalSamplingStepY = stepY; } /*! Set moving edge parameters. @@ -512,10 +462,7 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser \param fclip : New farClipping */ - inline void setFarClippingDistance(const double &fclip) - { - m_farClipping = fclip; - } + inline void setFarClippingDistance(const double &fclip) { m_farClipping = fclip; } /*! Set the size of a block. @@ -536,20 +483,14 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser \param mb = new maskBorder */ - inline void setKltMaskBorder(const unsigned int &mb) - { - m_kltMaskBorder = mb; - } + inline void setKltMaskBorder(const unsigned int &mb) { m_kltMaskBorder = mb; } /*! Set the maximum number of features for the KLT. \param mF : New maxFeatures */ - inline void setKltMaxFeatures(const unsigned int &mF) - { - m_kltMaxFeatures = mF; - } + inline void setKltMaxFeatures(const unsigned int &mF) { m_kltMaxFeatures = mF; } /*! Set the minimum distance between KLT points. @@ -563,10 +504,7 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser \param pL : New pyramidLevels */ - inline void setKltPyramidLevels(const unsigned int &pL) - { - m_kltPyramidLevels = pL; - } + inline void setKltPyramidLevels(const unsigned int &pL) { m_kltPyramidLevels = pL; } /*! Set the quality of the KLT. @@ -587,10 +525,7 @@ class VISP_EXPORT vpMbtXmlGenericParser : public vpXmlParser \param nclip : New nearClipping */ - inline void setNearClippingDistance(const double &nclip) - { - m_nearClipping = nclip; - } + inline void setNearClippingDistance(const double &nclip) { m_nearClipping = nclip; } void writeMainClass(xmlNodePtr node); //@} diff --git a/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp b/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp index f686574eff..2e6524d714 100644 --- a/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp +++ b/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp @@ -53,11 +53,9 @@ #endif vpMbDepthDenseTracker::vpMbDepthDenseTracker() - : m_depthDenseHiddenFacesDisplay(), m_depthDenseI_dummyVisibility(), - m_depthDenseListOfActiveFaces(), m_denseDepthNbFeatures(0), - m_depthDenseNormalFaces(), m_depthDenseSamplingStepX(2), - m_depthDenseSamplingStepY(2), m_error_depthDense(), m_L_depthDense(), - m_robust_depthDense(), m_w_depthDense(), m_weightedError_depthDense() + : m_depthDenseHiddenFacesDisplay(), m_depthDenseI_dummyVisibility(), m_depthDenseListOfActiveFaces(), + m_denseDepthNbFeatures(0), m_depthDenseNormalFaces(), m_depthDenseSamplingStepX(2), m_depthDenseSamplingStepY(2), + m_error_depthDense(), m_L_depthDense(), m_robust_depthDense(), m_w_depthDense(), m_weightedError_depthDense() #if DEBUG_DISPLAY_DEPTH_DENSE , m_debugDisp_depthDense(NULL), m_debugImage_depthDense() @@ -85,8 +83,7 @@ vpMbDepthDenseTracker::~vpMbDepthDenseTracker() #endif } -void vpMbDepthDenseTracker::addFace(vpMbtPolygon &polygon, - const bool alreadyClose) +void vpMbDepthDenseTracker::addFace(vpMbtPolygon &polygon, const bool alreadyClose) { if (polygon.nbpt < 3) { return; @@ -108,16 +105,14 @@ void vpMbDepthDenseTracker::addFace(vpMbtPolygon &polygon, unsigned int nbpt = polygon.getNbPoint(); if (nbpt > 0) { for (unsigned int i = 0; i < nbpt - 1; i++) { - normal_face->addLine(polygon.p[i], polygon.p[i + 1], - &m_depthDenseHiddenFacesDisplay, - polygon.getIndex(), polygon.getName()); + normal_face->addLine(polygon.p[i], polygon.p[i + 1], &m_depthDenseHiddenFacesDisplay, polygon.getIndex(), + polygon.getName()); } if (!alreadyClose) { // Add last line that closes the face - normal_face->addLine(polygon.p[nbpt - 1], polygon.p[0], - &m_depthDenseHiddenFacesDisplay, - polygon.getIndex(), polygon.getName()); + normal_face->addLine(polygon.p[nbpt - 1], polygon.p[0], &m_depthDenseHiddenFacesDisplay, polygon.getIndex(), + polygon.getName()); } } @@ -126,20 +121,17 @@ void vpMbDepthDenseTracker::addFace(vpMbtPolygon &polygon, pts[0] = polygon.p[0]; pts[1] = polygon.p[1]; pts[2] = polygon.p[2]; - normal_face->m_planeObject = - vpPlane(pts[0], pts[1], pts[2], vpPlane::object_frame); + normal_face->m_planeObject = vpPlane(pts[0], pts[1], pts[2], vpPlane::object_frame); m_depthDenseNormalFaces.push_back(normal_face); } -void vpMbDepthDenseTracker::computeVisibility(const unsigned int width, - const unsigned int height) +void vpMbDepthDenseTracker::computeVisibility(const unsigned int width, const unsigned int height) { m_depthDenseI_dummyVisibility.resize(height, width); bool changed = false; - faces.setVisible(m_depthDenseI_dummyVisibility, cam, cMo, angleAppears, - angleDisappears, changed); + faces.setVisible(m_depthDenseI_dummyVisibility, cam, cMo, angleAppears, angleDisappears, changed); if (useScanLine) { // if (clippingFlag <= 2) { @@ -152,8 +144,7 @@ void vpMbDepthDenseTracker::computeVisibility(const unsigned int width, m_depthDenseI_dummyVisibility.getHeight()); } - for (std::vector::const_iterator it = - m_depthDenseNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthDenseNormalFaces.begin(); it != m_depthDenseNormalFaces.end(); ++it) { vpMbtFaceDepthDense *face_normal = *it; face_normal->computeVisibility(); @@ -179,13 +170,11 @@ void vpMbDepthDenseTracker::computeVVS() vpVelocityTwistMatrix cVo; vpMatrix L_true, LVJ_true; - while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && - (iter < m_maxIter)) { + while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) { computeVVSInteractionMatrixAndResidu(); bool reStartFromLastIncrement = false; - computeVVSCheckLevenbergMarquardt(iter, m_error_depthDense, error_prev, - cMo_prev, mu, reStartFromLastIncrement); + computeVVSCheckLevenbergMarquardt(iter, m_error_depthDense, error_prev, cMo_prev, mu, reStartFromLastIncrement); if (!reStartFromLastIncrement) { computeVVSWeights(); @@ -214,8 +203,7 @@ void vpMbDepthDenseTracker::computeVVS() vpMatrix K; // kernel unsigned int rank = (m_L_depthDense * cVo).kernel(K); if (rank == 0) { - throw vpException(vpException::fatalError, - "Rank=0, cannot estimate the pose !"); + throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !"); } if (rank != 6) { @@ -231,8 +219,7 @@ void vpMbDepthDenseTracker::computeVVS() double num = 0.0, den = 0.0; for (unsigned int i = 0; i < m_L_depthDense.getRows(); i++) { // Compute weighted errors and stop criteria - m_weightedError_depthDense[i] = - m_w_depthDense[i] * m_error_depthDense[i]; + m_weightedError_depthDense[i] = m_w_depthDense[i] * m_error_depthDense[i]; num += m_w_depthDense[i] * vpMath::sqr(m_error_depthDense[i]); den += m_w_depthDense[i]; @@ -242,9 +229,8 @@ void vpMbDepthDenseTracker::computeVVS() } } - computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_depthDense, LTL, - m_weightedError_depthDense, m_error_depthDense, - error_prev, LTR, mu, v); + computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_depthDense, LTL, m_weightedError_depthDense, + m_error_depthDense, error_prev, LTR, mu, v); cMo_prev = cMo; cMo = vpExponentialMap::direct(v).inverse() * cMo; @@ -256,16 +242,14 @@ void vpMbDepthDenseTracker::computeVVS() iter++; } - computeCovarianceMatrixVVS(isoJoIdentity_, m_w_depthDense, cMo_prev, L_true, - LVJ_true, m_error_depthDense); + computeCovarianceMatrixVVS(isoJoIdentity_, m_w_depthDense, cMo_prev, L_true, LVJ_true, m_error_depthDense); } void vpMbDepthDenseTracker::computeVVSInit() { m_denseDepthNbFeatures = 0; - for (std::vector::const_iterator it = - m_depthDenseListOfActiveFaces.begin(); + for (std::vector::const_iterator it = m_depthDenseListOfActiveFaces.begin(); it != m_depthDenseListOfActiveFaces.end(); ++it) { vpMbtFaceDepthDense *face = *it; m_denseDepthNbFeatures += face->getNbFeatures(); @@ -282,8 +266,7 @@ void vpMbDepthDenseTracker::computeVVSInit() void vpMbDepthDenseTracker::computeVVSInteractionMatrixAndResidu() { unsigned int start_index = 0; - for (std::vector::const_iterator it = - m_depthDenseListOfActiveFaces.begin(); + for (std::vector::const_iterator it = m_depthDenseListOfActiveFaces.begin(); it != m_depthDenseListOfActiveFaces.end(); ++it) { vpMbtFaceDepthDense *face = *it; @@ -304,29 +287,23 @@ void vpMbDepthDenseTracker::computeVVSWeights() m_robust_depthDense.MEstimator(m_error_depthDense, m_w_depthDense, 1e-3); } -void vpMbDepthDenseTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, - const vpColor &col, - const unsigned int thickness, +void vpMbDepthDenseTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { vpCameraParameters c = cam_; bool changed = false; - m_depthDenseHiddenFacesDisplay.setVisible(I, c, cMo_, angleAppears, - angleDisappears, changed); + m_depthDenseHiddenFacesDisplay.setVisible(I, c, cMo_, angleAppears, angleDisappears, changed); if (useScanLine) { c.computeFov(I.getWidth(), I.getHeight()); m_depthDenseHiddenFacesDisplay.computeClippedPolygons(cMo_, c); - m_depthDenseHiddenFacesDisplay.computeScanLineRender(c, I.getWidth(), - I.getHeight()); + m_depthDenseHiddenFacesDisplay.computeScanLineRender(c, I.getWidth(), I.getHeight()); } - for (std::vector::const_iterator it = - m_depthDenseNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthDenseNormalFaces.begin(); it != m_depthDenseNormalFaces.end(); ++it) { vpMbtFaceDepthDense *face_normal = *it; face_normal->display(I, cMo_, c, col, thickness, displayFullModel); @@ -337,11 +314,8 @@ void vpMbDepthDenseTracker::display(const vpImage &I, } } -void vpMbDepthDenseTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, - const vpColor &col, - const unsigned int thickness, +void vpMbDepthDenseTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { vpCameraParameters c = cam_; @@ -349,19 +323,16 @@ void vpMbDepthDenseTracker::display(const vpImage &I, bool changed = false; vpImage I_dummy; vpImageConvert::convert(I, I_dummy); - m_depthDenseHiddenFacesDisplay.setVisible(I_dummy, c, cMo_, angleAppears, - angleDisappears, changed); + m_depthDenseHiddenFacesDisplay.setVisible(I_dummy, c, cMo_, angleAppears, angleDisappears, changed); if (useScanLine) { c.computeFov(I.getWidth(), I.getHeight()); m_depthDenseHiddenFacesDisplay.computeClippedPolygons(cMo_, c); - m_depthDenseHiddenFacesDisplay.computeScanLineRender(c, I.getWidth(), - I.getHeight()); + m_depthDenseHiddenFacesDisplay.computeScanLineRender(c, I.getWidth(), I.getHeight()); } - for (std::vector::const_iterator it = - m_depthDenseNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthDenseNormalFaces.begin(); it != m_depthDenseNormalFaces.end(); ++it) { vpMbtFaceDepthDense *face_normal = *it; face_normal->display(I, cMo_, c, col, thickness, displayFullModel); @@ -380,8 +351,7 @@ void vpMbDepthDenseTracker::init(const vpImage &I) bool reInitialisation = false; if (!useOgre) { - faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); } else { #ifdef VISP_HAVE_OGRE if (!faces.isOgreInitialised()) { @@ -394,11 +364,9 @@ void vpMbDepthDenseTracker::init(const vpImage &I) ogreShowConfigDialog = false; } - faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); #else - faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); #endif } @@ -421,14 +389,11 @@ void vpMbDepthDenseTracker::loadConfigFile(const std::string &configFile) xmlp.setDepthDenseSamplingStepY(m_depthDenseSamplingStepY); try { - std::cout - << " *********** Parsing XML for Mb Depth Dense Tracker ************ " - << std::endl; + std::cout << " *********** Parsing XML for Mb Depth Dense Tracker ************ " << std::endl; xmlp.parse(configFile); } catch (vpException &e) { std::cerr << "Exception: " << e.what() << std::endl; - throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", - configFile.c_str()); + throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str()); } vpCameraParameters camera; @@ -447,18 +412,14 @@ void vpMbDepthDenseTracker::loadConfigFile(const std::string &configFile) if (xmlp.getFovClipping()) setClipping(clippingFlag | vpPolygon3D::FOV_CLIPPING); - setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), - xmlp.getDepthDenseSamplingStepY()); + setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), xmlp.getDepthDenseSamplingStepY()); #else - std::cerr << "You need the libXML2 to read the config file " << configFile - << std::endl; + std::cerr << "You need the libXML2 to read the config file " << configFile << std::endl; #endif } -void vpMbDepthDenseTracker::reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbDepthDenseTracker::reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose) { cMo.eye(); @@ -474,10 +435,9 @@ void vpMbDepthDenseTracker::reInitModel(const vpImage &I, } #if defined(VISP_HAVE_PCL) -void vpMbDepthDenseTracker::reInitModel( - const pcl::PointCloud::ConstPtr &point_cloud, - const std::string &cad_name, const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbDepthDenseTracker::reInitModel(const pcl::PointCloud::ConstPtr &point_cloud, + const std::string &cad_name, const vpHomogeneousMatrix &cMo_, + const bool verbose) { vpImage I_dummy(point_cloud->height, point_cloud->width); reInitModel(I_dummy, cad_name, cMo_, verbose); @@ -489,8 +449,7 @@ void vpMbDepthDenseTracker::resetTracker() { cMo.eye(); - for (std::vector::iterator it = - m_depthDenseNormalFaces.begin(); + for (std::vector::iterator it = m_depthDenseNormalFaces.begin(); it != m_depthDenseNormalFaces.end(); ++it) { vpMbtFaceDepthDense *normal_face = *it; delete normal_face; @@ -531,17 +490,15 @@ void vpMbDepthDenseTracker::setOgreVisibilityTest(const bool &v) #endif } -void vpMbDepthDenseTracker::setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo) +void vpMbDepthDenseTracker::setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo) { cMo = cdMo; init(I); } #if defined(VISP_HAVE_PCL) -void vpMbDepthDenseTracker::setPose( - const pcl::PointCloud::ConstPtr &point_cloud, - const vpHomogeneousMatrix &cdMo) +void vpMbDepthDenseTracker::setPose(const pcl::PointCloud::ConstPtr &point_cloud, + const vpHomogeneousMatrix &cdMo) { vpImage I_dummy(point_cloud->height, point_cloud->width); cMo = cdMo; @@ -553,8 +510,7 @@ void vpMbDepthDenseTracker::setScanLineVisibilityTest(const bool &v) { vpMbTracker::setScanLineVisibilityTest(v); - for (std::vector::const_iterator it = - m_depthDenseNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthDenseNormalFaces.begin(); it != m_depthDenseNormalFaces.end(); ++it) { (*it)->setScanLineVisibilityTest(v); } @@ -563,24 +519,21 @@ void vpMbDepthDenseTracker::setScanLineVisibilityTest(const bool &v) void vpMbDepthDenseTracker::testTracking() {} #ifdef VISP_HAVE_PCL -void vpMbDepthDenseTracker::segmentPointCloud( - const pcl::PointCloud::ConstPtr &point_cloud) +void vpMbDepthDenseTracker::segmentPointCloud(const pcl::PointCloud::ConstPtr &point_cloud) { m_depthDenseListOfActiveFaces.clear(); #if DEBUG_DISPLAY_DEPTH_DENSE if (!m_debugDisp_depthDense->isInitialised()) { m_debugImage_depthDense.resize(point_cloud->height, point_cloud->width); - m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, - "Debug display dense depth tracker"); + m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, "Debug display dense depth tracker"); } m_debugImage_depthDense = 0; std::vector > roiPts_vec; #endif - for (std::vector::iterator it = - m_depthDenseNormalFaces.begin(); + for (std::vector::iterator it = m_depthDenseNormalFaces.begin(); it != m_depthDenseNormalFaces.end(); ++it) { vpMbtFaceDepthDense *face = *it; @@ -588,9 +541,7 @@ void vpMbDepthDenseTracker::segmentPointCloud( #if DEBUG_DISPLAY_DEPTH_DENSE std::vector > roiPts_vec_; #endif - if (face->computeDesiredFeatures(cMo, point_cloud, - m_depthDenseSamplingStepX, - m_depthDenseSamplingStepY + if (face->computeDesiredFeatures(cMo, point_cloud, m_depthDenseSamplingStepX, m_depthDenseSamplingStepY #if DEBUG_DISPLAY_DEPTH_DENSE , m_debugImage_depthDense, roiPts_vec_ @@ -599,8 +550,7 @@ void vpMbDepthDenseTracker::segmentPointCloud( m_depthDenseListOfActiveFaces.push_back(*it); #if DEBUG_DISPLAY_DEPTH_DENSE - roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), - roiPts_vec_.end()); + roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end()); #endif } } @@ -614,11 +564,9 @@ void vpMbDepthDenseTracker::segmentPointCloud( continue; for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) { - vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], - roiPts_vec[i][j + 1], vpColor::red, 2); + vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2); } - vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], - roiPts_vec[i][roiPts_vec[i].size() - 1], + vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1], vpColor::red, 2); } @@ -627,25 +575,22 @@ void vpMbDepthDenseTracker::segmentPointCloud( } #endif -void vpMbDepthDenseTracker::segmentPointCloud( - const std::vector &point_cloud, const unsigned int width, - const unsigned int height) +void vpMbDepthDenseTracker::segmentPointCloud(const std::vector &point_cloud, const unsigned int width, + const unsigned int height) { m_depthDenseListOfActiveFaces.clear(); #if DEBUG_DISPLAY_DEPTH_DENSE if (!m_debugDisp_depthDense->isInitialised()) { m_debugImage_depthDense.resize(height, width); - m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, - "Debug display dense depth tracker"); + m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, "Debug display dense depth tracker"); } m_debugImage_depthDense = 0; std::vector > roiPts_vec; #endif - for (std::vector::iterator it = - m_depthDenseNormalFaces.begin(); + for (std::vector::iterator it = m_depthDenseNormalFaces.begin(); it != m_depthDenseNormalFaces.end(); ++it) { vpMbtFaceDepthDense *face = *it; @@ -653,8 +598,7 @@ void vpMbDepthDenseTracker::segmentPointCloud( #if DEBUG_DISPLAY_DEPTH_DENSE std::vector > roiPts_vec_; #endif - if (face->computeDesiredFeatures(cMo, width, height, point_cloud, - m_depthDenseSamplingStepX, + if (face->computeDesiredFeatures(cMo, width, height, point_cloud, m_depthDenseSamplingStepX, m_depthDenseSamplingStepY #if DEBUG_DISPLAY_DEPTH_DENSE , @@ -664,8 +608,7 @@ void vpMbDepthDenseTracker::segmentPointCloud( m_depthDenseListOfActiveFaces.push_back(*it); #if DEBUG_DISPLAY_DEPTH_DENSE - roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), - roiPts_vec_.end()); + roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end()); #endif } } @@ -679,11 +622,9 @@ void vpMbDepthDenseTracker::segmentPointCloud( continue; for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) { - vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], - roiPts_vec[i][j + 1], vpColor::red, 2); + vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2); } - vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], - roiPts_vec[i][roiPts_vec[i].size() - 1], + vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1], vpColor::red, 2); } @@ -691,23 +632,19 @@ void vpMbDepthDenseTracker::segmentPointCloud( #endif } -void vpMbDepthDenseTracker::setCameraParameters( - const vpCameraParameters &camera) +void vpMbDepthDenseTracker::setCameraParameters(const vpCameraParameters &camera) { this->cam = camera; - for (std::vector::const_iterator it = - m_depthDenseNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthDenseNormalFaces.begin(); it != m_depthDenseNormalFaces.end(); ++it) { (*it)->setCameraParameters(camera); } } -void vpMbDepthDenseTracker::setDepthDenseFilteringMaxDistance( - const double maxDistance) +void vpMbDepthDenseTracker::setDepthDenseFilteringMaxDistance(const double maxDistance) { - for (std::vector::const_iterator it = - m_depthDenseNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthDenseNormalFaces.begin(); it != m_depthDenseNormalFaces.end(); ++it) { (*it)->setDepthDenseFilteringMaxDistance(maxDistance); } @@ -715,33 +652,28 @@ void vpMbDepthDenseTracker::setDepthDenseFilteringMaxDistance( void vpMbDepthDenseTracker::setDepthDenseFilteringMethod(const int method) { - for (std::vector::const_iterator it = - m_depthDenseNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthDenseNormalFaces.begin(); it != m_depthDenseNormalFaces.end(); ++it) { (*it)->setDepthDenseFilteringMethod(method); } } -void vpMbDepthDenseTracker::setDepthDenseFilteringMinDistance( - const double minDistance) +void vpMbDepthDenseTracker::setDepthDenseFilteringMinDistance(const double minDistance) { - for (std::vector::const_iterator it = - m_depthDenseNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthDenseNormalFaces.begin(); it != m_depthDenseNormalFaces.end(); ++it) { (*it)->setDepthDenseFilteringMinDistance(minDistance); } } -void vpMbDepthDenseTracker::setDepthDenseFilteringOccupancyRatio( - const double occupancyRatio) +void vpMbDepthDenseTracker::setDepthDenseFilteringOccupancyRatio(const double occupancyRatio) { if (occupancyRatio < 0.0 || occupancyRatio > 1.0) { std::cerr << "occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl; return; } - for (std::vector::const_iterator it = - m_depthDenseNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthDenseNormalFaces.begin(); it != m_depthDenseNormalFaces.end(); ++it) { (*it)->setDepthDenseFilteringOccupancyRatio(occupancyRatio); } @@ -749,13 +681,11 @@ void vpMbDepthDenseTracker::setDepthDenseFilteringOccupancyRatio( void vpMbDepthDenseTracker::track(const vpImage &) { - throw vpException(vpException::fatalError, - "Cannot track with a grayscale image!"); + throw vpException(vpException::fatalError, "Cannot track with a grayscale image!"); } #ifdef VISP_HAVE_PCL -void vpMbDepthDenseTracker::track( - const pcl::PointCloud::ConstPtr &point_cloud) +void vpMbDepthDenseTracker::track(const pcl::PointCloud::ConstPtr &point_cloud) { segmentPointCloud(point_cloud); @@ -765,8 +695,7 @@ void vpMbDepthDenseTracker::track( } #endif -void vpMbDepthDenseTracker::track(const std::vector &point_cloud, - const unsigned int width, +void vpMbDepthDenseTracker::track(const std::vector &point_cloud, const unsigned int width, const unsigned int height) { segmentPointCloud(point_cloud, width, height); @@ -776,35 +705,18 @@ void vpMbDepthDenseTracker::track(const std::vector &point_cloud, computeVisibility(width, height); } -void vpMbDepthDenseTracker::initCircle(const vpPoint & /*p1*/, - const vpPoint & /*p2*/, - const vpPoint & /*p3*/, - const double /*radius*/, - const int /*idFace*/, - const std::string & /*name*/) +void vpMbDepthDenseTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/, + const double /*radius*/, const int /*idFace*/, const std::string & /*name*/) { - throw vpException( - vpException::fatalError, - "vpMbDepthDenseTracker::initCircle() should not be called!"); + throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCircle() should not be called!"); } -void vpMbDepthDenseTracker::initCylinder(const vpPoint & /*p1*/, - const vpPoint & /*p2*/, - const double /*radius*/, - const int /*idFace*/, - const std::string & /*name*/) +void vpMbDepthDenseTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const double /*radius*/, + const int /*idFace*/, const std::string & /*name*/) { - throw vpException( - vpException::fatalError, - "vpMbDepthDenseTracker::initCylinder() should not be called!"); + throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCylinder() should not be called!"); } -void vpMbDepthDenseTracker::initFaceFromCorners(vpMbtPolygon &polygon) -{ - addFace(polygon, false); -} +void vpMbDepthDenseTracker::initFaceFromCorners(vpMbtPolygon &polygon) { addFace(polygon, false); } -void vpMbDepthDenseTracker::initFaceFromLines(vpMbtPolygon &polygon) -{ - addFace(polygon, true); -} +void vpMbDepthDenseTracker::initFaceFromLines(vpMbtPolygon &polygon) { addFace(polygon, true); } diff --git a/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp b/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp index e90b868e3b..fba071ced6 100644 --- a/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp +++ b/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp @@ -53,16 +53,12 @@ #endif vpMbDepthNormalTracker::vpMbDepthNormalTracker() - : m_depthNormalFeatureEstimationMethod( - vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION), - m_depthNormalHiddenFacesDisplay(), m_depthNormalI_dummyVisibility(), - m_depthNormalListOfActiveFaces(), m_depthNormalListOfDesiredFeatures(), - m_depthNormalFaces(), m_depthNormalPclPlaneEstimationMethod(2), - m_depthNormalPclPlaneEstimationRansacMaxIter(200), - m_depthNormalPclPlaneEstimationRansacThreshold(0.001), - m_depthNormalSamplingStepX(2), m_depthNormalSamplingStepY(2), - m_depthNormalUseRobust(false), m_error_depthNormal(), m_L_depthNormal(), - m_robust_depthNormal(), m_w_depthNormal(), m_weightedError_depthNormal() + : m_depthNormalFeatureEstimationMethod(vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION), + m_depthNormalHiddenFacesDisplay(), m_depthNormalI_dummyVisibility(), m_depthNormalListOfActiveFaces(), + m_depthNormalListOfDesiredFeatures(), m_depthNormalFaces(), m_depthNormalPclPlaneEstimationMethod(2), + m_depthNormalPclPlaneEstimationRansacMaxIter(200), m_depthNormalPclPlaneEstimationRansacThreshold(0.001), + m_depthNormalSamplingStepX(2), m_depthNormalSamplingStepY(2), m_depthNormalUseRobust(false), m_error_depthNormal(), + m_L_depthNormal(), m_robust_depthNormal(), m_w_depthNormal(), m_weightedError_depthNormal() #if DEBUG_DISPLAY_DEPTH_NORMAL , m_debugDisp_depthNormal(NULL), m_debugImage_depthNormal() @@ -86,8 +82,7 @@ vpMbDepthNormalTracker::~vpMbDepthNormalTracker() } } -void vpMbDepthNormalTracker::addFace(vpMbtPolygon &polygon, - const bool alreadyClose) +void vpMbDepthNormalTracker::addFace(vpMbtPolygon &polygon, const bool alreadyClose) { if (polygon.nbpt < 3) { return; @@ -104,29 +99,23 @@ void vpMbDepthNormalTracker::addFace(vpMbtPolygon &polygon, normal_face->m_clippingFlag = clippingFlag; normal_face->m_distNearClip = distNearClip; normal_face->m_distFarClip = distFarClip; - normal_face->setFeatureEstimationMethod( - m_depthNormalFeatureEstimationMethod); - normal_face->setPclPlaneEstimationMethod( - m_depthNormalPclPlaneEstimationMethod); - normal_face->setPclPlaneEstimationRansacMaxIter( - m_depthNormalPclPlaneEstimationRansacMaxIter); - normal_face->setPclPlaneEstimationRansacThreshold( - m_depthNormalPclPlaneEstimationRansacThreshold); + normal_face->setFeatureEstimationMethod(m_depthNormalFeatureEstimationMethod); + normal_face->setPclPlaneEstimationMethod(m_depthNormalPclPlaneEstimationMethod); + normal_face->setPclPlaneEstimationRansacMaxIter(m_depthNormalPclPlaneEstimationRansacMaxIter); + normal_face->setPclPlaneEstimationRansacThreshold(m_depthNormalPclPlaneEstimationRansacThreshold); // Add lines that compose the face unsigned int nbpt = polygon.getNbPoint(); if (nbpt > 0) { for (unsigned int i = 0; i < nbpt - 1; i++) { - normal_face->addLine(polygon.p[i], polygon.p[i + 1], - &m_depthNormalHiddenFacesDisplay, - polygon.getIndex(), polygon.getName()); + normal_face->addLine(polygon.p[i], polygon.p[i + 1], &m_depthNormalHiddenFacesDisplay, polygon.getIndex(), + polygon.getName()); } if (!alreadyClose) { // Add last line that closes the face - normal_face->addLine(polygon.p[nbpt - 1], polygon.p[0], - &m_depthNormalHiddenFacesDisplay, - polygon.getIndex(), polygon.getName()); + normal_face->addLine(polygon.p[nbpt - 1], polygon.p[0], &m_depthNormalHiddenFacesDisplay, polygon.getIndex(), + polygon.getName()); } } @@ -135,20 +124,17 @@ void vpMbDepthNormalTracker::addFace(vpMbtPolygon &polygon, pts[0] = polygon.p[0]; pts[1] = polygon.p[1]; pts[2] = polygon.p[2]; - normal_face->m_planeObject = - vpPlane(pts[0], pts[1], pts[2], vpPlane::object_frame); + normal_face->m_planeObject = vpPlane(pts[0], pts[1], pts[2], vpPlane::object_frame); m_depthNormalFaces.push_back(normal_face); } -void vpMbDepthNormalTracker::computeVisibility(const unsigned int width, - const unsigned int height) +void vpMbDepthNormalTracker::computeVisibility(const unsigned int width, const unsigned int height) { m_depthNormalI_dummyVisibility.resize(height, width); bool changed = false; - faces.setVisible(m_depthNormalI_dummyVisibility, cam, cMo, angleAppears, - angleDisappears, changed); + faces.setVisible(m_depthNormalI_dummyVisibility, cam, cMo, angleAppears, angleDisappears, changed); if (useScanLine) { // if (clippingFlag <= 2) { @@ -157,13 +143,11 @@ void vpMbDepthNormalTracker::computeVisibility(const unsigned int width, // } faces.computeClippedPolygons(cMo, cam); - faces.computeScanLineRender(cam, - m_depthNormalI_dummyVisibility.getWidth(), + faces.computeScanLineRender(cam, m_depthNormalI_dummyVisibility.getWidth(), m_depthNormalI_dummyVisibility.getHeight()); } - for (std::vector::const_iterator it = - m_depthNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); ++it) { vpMbtFaceDepthNormal *face_normal = *it; face_normal->computeVisibility(); @@ -177,8 +161,7 @@ void vpMbDepthNormalTracker::computeVVS() unsigned int iter = 0; computeVVSInit(); - unsigned int nb_features = - (unsigned int)(3 * m_depthNormalListOfDesiredFeatures.size()); + unsigned int nb_features = (unsigned int)(3 * m_depthNormalListOfDesiredFeatures.size()); vpColVector error_prev(nb_features); vpMatrix LTL; @@ -191,18 +174,15 @@ void vpMbDepthNormalTracker::computeVVS() vpVelocityTwistMatrix cVo; vpMatrix L_true, LVJ_true; - while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && - (iter < m_maxIter)) { + while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) { computeVVSInteractionMatrixAndResidu(); bool reStartFromLastIncrement = false; - computeVVSCheckLevenbergMarquardt(iter, m_error_depthNormal, error_prev, - cMo_prev, mu, reStartFromLastIncrement); + computeVVSCheckLevenbergMarquardt(iter, m_error_depthNormal, error_prev, cMo_prev, mu, reStartFromLastIncrement); if (!reStartFromLastIncrement) { if (m_depthNormalUseRobust) - computeVVSWeights(m_robust_depthNormal, m_error_depthNormal, - m_w_depthNormal); + computeVVSWeights(m_robust_depthNormal, m_error_depthNormal, m_w_depthNormal); if (computeCovariance) { L_true = m_L_depthNormal; @@ -228,8 +208,7 @@ void vpMbDepthNormalTracker::computeVVS() vpMatrix K; // kernel unsigned int rank = (m_L_depthNormal * cVo).kernel(K); if (rank == 0) { - throw vpException(vpException::fatalError, - "Rank=0, cannot estimate the pose !"); + throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !"); } if (rank != 6) { @@ -245,8 +224,7 @@ void vpMbDepthNormalTracker::computeVVS() double num = 0.0, den = 0.0; for (unsigned int i = 0; i < m_L_depthNormal.getRows(); i++) { // Compute weighted errors and stop criteria - m_weightedError_depthNormal[i] = - m_w_depthNormal[i] * m_error_depthNormal[i]; + m_weightedError_depthNormal[i] = m_w_depthNormal[i] * m_error_depthNormal[i]; num += m_w_depthNormal[i] * vpMath::sqr(m_error_depthNormal[i]); den += m_w_depthNormal[i]; @@ -256,8 +234,7 @@ void vpMbDepthNormalTracker::computeVVS() } } - computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_depthNormal, LTL, - m_weightedError_depthNormal, + computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_depthNormal, LTL, m_weightedError_depthNormal, m_error_depthNormal, error_prev, LTR, mu, v); cMo_prev = cMo; @@ -270,14 +247,12 @@ void vpMbDepthNormalTracker::computeVVS() iter++; } - computeCovarianceMatrixVVS(isoJoIdentity_, m_w_depthNormal, cMo_prev, - L_true, LVJ_true, m_error_depthNormal); + computeCovarianceMatrixVVS(isoJoIdentity_, m_w_depthNormal, cMo_prev, L_true, LVJ_true, m_error_depthNormal); } void vpMbDepthNormalTracker::computeVVSInit() { - unsigned int nb_features = - (unsigned int)(3 * m_depthNormalListOfDesiredFeatures.size()); + unsigned int nb_features = (unsigned int)(3 * m_depthNormalListOfDesiredFeatures.size()); m_L_depthNormal.resize(nb_features, 6, false, false); m_error_depthNormal.resize(nb_features, false); @@ -293,15 +268,13 @@ void vpMbDepthNormalTracker::computeVVSInit() void vpMbDepthNormalTracker::computeVVSInteractionMatrixAndResidu() { unsigned int cpt = 0; - for (std::vector::const_iterator it = - m_depthNormalListOfActiveFaces.begin(); + for (std::vector::const_iterator it = m_depthNormalListOfActiveFaces.begin(); it != m_depthNormalListOfActiveFaces.end(); ++it) { vpMatrix L_face; vpColVector features_face; (*it)->computeInteractionMatrix(cMo, L_face, features_face); - vpColVector face_error = - features_face - m_depthNormalListOfDesiredFeatures[(size_t)cpt]; + vpColVector face_error = features_face - m_depthNormalListOfDesiredFeatures[(size_t)cpt]; m_error_depthNormal.insert(cpt * 3, face_error); m_L_depthNormal.insert(L_face, cpt * 3, 0); @@ -310,29 +283,23 @@ void vpMbDepthNormalTracker::computeVVSInteractionMatrixAndResidu() } } -void vpMbDepthNormalTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, - const vpColor &col, - const unsigned int thickness, +void vpMbDepthNormalTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { vpCameraParameters c = cam_; bool changed = false; - m_depthNormalHiddenFacesDisplay.setVisible(I, c, cMo_, angleAppears, - angleDisappears, changed); + m_depthNormalHiddenFacesDisplay.setVisible(I, c, cMo_, angleAppears, angleDisappears, changed); if (useScanLine) { c.computeFov(I.getWidth(), I.getHeight()); m_depthNormalHiddenFacesDisplay.computeClippedPolygons(cMo_, c); - m_depthNormalHiddenFacesDisplay.computeScanLineRender(c, I.getWidth(), - I.getHeight()); + m_depthNormalHiddenFacesDisplay.computeScanLineRender(c, I.getWidth(), I.getHeight()); } - for (std::vector::const_iterator it = - m_depthNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); ++it) { vpMbtFaceDepthNormal *face_normal = *it; face_normal->display(I, cMo_, c, col, thickness, displayFullModel); @@ -343,11 +310,8 @@ void vpMbDepthNormalTracker::display(const vpImage &I, } } -void vpMbDepthNormalTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, - const vpColor &col, - const unsigned int thickness, +void vpMbDepthNormalTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { vpCameraParameters c = cam_; @@ -355,19 +319,16 @@ void vpMbDepthNormalTracker::display(const vpImage &I, bool changed = false; vpImage I_dummy; vpImageConvert::convert(I, I_dummy); - m_depthNormalHiddenFacesDisplay.setVisible(I_dummy, c, cMo_, angleAppears, - angleDisappears, changed); + m_depthNormalHiddenFacesDisplay.setVisible(I_dummy, c, cMo_, angleAppears, angleDisappears, changed); if (useScanLine) { c.computeFov(I.getWidth(), I.getHeight()); m_depthNormalHiddenFacesDisplay.computeClippedPolygons(cMo_, c); - m_depthNormalHiddenFacesDisplay.computeScanLineRender(c, I.getWidth(), - I.getHeight()); + m_depthNormalHiddenFacesDisplay.computeScanLineRender(c, I.getWidth(), I.getHeight()); } - for (std::vector::const_iterator it = - m_depthNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); ++it) { vpMbtFaceDepthNormal *face_normal = *it; face_normal->display(I, cMo_, c, col, thickness, displayFullModel); @@ -386,8 +347,7 @@ void vpMbDepthNormalTracker::init(const vpImage &I) bool reInitialisation = false; if (!useOgre) { - faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); } else { #ifdef VISP_HAVE_OGRE if (!faces.isOgreInitialised()) { @@ -400,11 +360,9 @@ void vpMbDepthNormalTracker::init(const vpImage &I) ogreShowConfigDialog = false; } - faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); #else - faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); #endif } @@ -423,25 +381,19 @@ void vpMbDepthNormalTracker::loadConfigFile(const std::string &configFile) xmlp.setAngleAppear(vpMath::deg(angleAppears)); xmlp.setAngleDisappear(vpMath::deg(angleDisappears)); - xmlp.setDepthNormalFeatureEstimationMethod( - m_depthNormalFeatureEstimationMethod); - xmlp.setDepthNormalPclPlaneEstimationMethod( - m_depthNormalPclPlaneEstimationMethod); - xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter( - m_depthNormalPclPlaneEstimationRansacMaxIter); - xmlp.setDepthNormalPclPlaneEstimationRansacThreshold( - m_depthNormalPclPlaneEstimationRansacThreshold); + xmlp.setDepthNormalFeatureEstimationMethod(m_depthNormalFeatureEstimationMethod); + xmlp.setDepthNormalPclPlaneEstimationMethod(m_depthNormalPclPlaneEstimationMethod); + xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter(m_depthNormalPclPlaneEstimationRansacMaxIter); + xmlp.setDepthNormalPclPlaneEstimationRansacThreshold(m_depthNormalPclPlaneEstimationRansacThreshold); xmlp.setDepthNormalSamplingStepX(m_depthNormalSamplingStepX); xmlp.setDepthNormalSamplingStepY(m_depthNormalSamplingStepY); try { - std::cout << " *********** Parsing XML for Mb Depth Tracker ************ " - << std::endl; + std::cout << " *********** Parsing XML for Mb Depth Tracker ************ " << std::endl; xmlp.parse(configFile); } catch (vpException &e) { std::cerr << "Exception: " << e.what() << std::endl; - throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", - configFile.c_str()); + throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str()); } vpCameraParameters camera; @@ -460,26 +412,18 @@ void vpMbDepthNormalTracker::loadConfigFile(const std::string &configFile) if (xmlp.getFovClipping()) setClipping(clippingFlag | vpPolygon3D::FOV_CLIPPING); - setDepthNormalFeatureEstimationMethod( - xmlp.getDepthNormalFeatureEstimationMethod()); - setDepthNormalPclPlaneEstimationMethod( - xmlp.getDepthNormalPclPlaneEstimationMethod()); - setDepthNormalPclPlaneEstimationRansacMaxIter( - xmlp.getDepthNormalPclPlaneEstimationRansacMaxIter()); - setDepthNormalPclPlaneEstimationRansacThreshold( - xmlp.getDepthNormalPclPlaneEstimationRansacThreshold()); - setDepthNormalSamplingStep(xmlp.getDepthNormalSamplingStepX(), - xmlp.getDepthNormalSamplingStepY()); + setDepthNormalFeatureEstimationMethod(xmlp.getDepthNormalFeatureEstimationMethod()); + setDepthNormalPclPlaneEstimationMethod(xmlp.getDepthNormalPclPlaneEstimationMethod()); + setDepthNormalPclPlaneEstimationRansacMaxIter(xmlp.getDepthNormalPclPlaneEstimationRansacMaxIter()); + setDepthNormalPclPlaneEstimationRansacThreshold(xmlp.getDepthNormalPclPlaneEstimationRansacThreshold()); + setDepthNormalSamplingStep(xmlp.getDepthNormalSamplingStepX(), xmlp.getDepthNormalSamplingStepY()); #else - std::cerr << "You need the libXML2 to read the config file " << configFile - << std::endl; + std::cerr << "You need the libXML2 to read the config file " << configFile << std::endl; #endif } -void vpMbDepthNormalTracker::reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbDepthNormalTracker::reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose) { cMo.eye(); @@ -495,10 +439,9 @@ void vpMbDepthNormalTracker::reInitModel(const vpImage &I, } #if defined(VISP_HAVE_PCL) -void vpMbDepthNormalTracker::reInitModel( - const pcl::PointCloud::ConstPtr &point_cloud, - const std::string &cad_name, const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbDepthNormalTracker::reInitModel(const pcl::PointCloud::ConstPtr &point_cloud, + const std::string &cad_name, const vpHomogeneousMatrix &cMo_, + const bool verbose) { vpImage I_dummy(point_cloud->height, point_cloud->width); reInitModel(I_dummy, cad_name, cMo_, verbose); @@ -510,9 +453,8 @@ void vpMbDepthNormalTracker::resetTracker() { cMo.eye(); - for (std::vector::iterator it = - m_depthNormalFaces.begin(); - it != m_depthNormalFaces.end(); ++it) { + for (std::vector::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); + ++it) { vpMbtFaceDepthNormal *normal_face = *it; delete normal_face; normal_face = NULL; @@ -552,17 +494,15 @@ void vpMbDepthNormalTracker::setOgreVisibilityTest(const bool &v) #endif } -void vpMbDepthNormalTracker::setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo) +void vpMbDepthNormalTracker::setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo) { cMo = cdMo; init(I); } #if defined(VISP_HAVE_PCL) -void vpMbDepthNormalTracker::setPose( - const pcl::PointCloud::ConstPtr &point_cloud, - const vpHomogeneousMatrix &cdMo) +void vpMbDepthNormalTracker::setPose(const pcl::PointCloud::ConstPtr &point_cloud, + const vpHomogeneousMatrix &cdMo) { vpImage I_dummy(point_cloud->height, point_cloud->width); cMo = cdMo; @@ -574,8 +514,7 @@ void vpMbDepthNormalTracker::setScanLineVisibilityTest(const bool &v) { vpMbTracker::setScanLineVisibilityTest(v); - for (std::vector::const_iterator it = - m_depthNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); ++it) { (*it)->setScanLineVisibilityTest(v); } @@ -584,8 +523,7 @@ void vpMbDepthNormalTracker::setScanLineVisibilityTest(const bool &v) void vpMbDepthNormalTracker::testTracking() {} #ifdef VISP_HAVE_PCL -void vpMbDepthNormalTracker::segmentPointCloud( - const pcl::PointCloud::ConstPtr &point_cloud) +void vpMbDepthNormalTracker::segmentPointCloud(const pcl::PointCloud::ConstPtr &point_cloud) { m_depthNormalListOfActiveFaces.clear(); m_depthNormalListOfDesiredFeatures.clear(); @@ -593,17 +531,15 @@ void vpMbDepthNormalTracker::segmentPointCloud( #if DEBUG_DISPLAY_DEPTH_NORMAL if (!m_debugDisp_depthNormal->isInitialised()) { m_debugImage_depthNormal.resize(point_cloud->height, point_cloud->width); - m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0, - "Debug display normal depth tracker"); + m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0, "Debug display normal depth tracker"); } m_debugImage_depthNormal = 0; std::vector > roiPts_vec; #endif - for (std::vector::iterator it = - m_depthNormalFaces.begin(); - it != m_depthNormalFaces.end(); ++it) { + for (std::vector::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); + ++it) { vpMbtFaceDepthNormal *face = *it; if (face->isVisible()) { @@ -612,21 +548,18 @@ void vpMbDepthNormalTracker::segmentPointCloud( #if DEBUG_DISPLAY_DEPTH_NORMAL std::vector > roiPts_vec_; #endif - if (face->computeDesiredFeatures( - cMo, point_cloud->width, point_cloud->height, point_cloud, - desired_features, m_depthNormalSamplingStepX, - m_depthNormalSamplingStepY + if (face->computeDesiredFeatures(cMo, point_cloud->width, point_cloud->height, point_cloud, desired_features, + m_depthNormalSamplingStepX, m_depthNormalSamplingStepY #if DEBUG_DISPLAY_DEPTH_NORMAL - , - m_debugImage_depthNormal, roiPts_vec_ + , + m_debugImage_depthNormal, roiPts_vec_ #endif - )) { + )) { m_depthNormalListOfDesiredFeatures.push_back(desired_features); m_depthNormalListOfActiveFaces.push_back(face); #if DEBUG_DISPLAY_DEPTH_NORMAL - roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), - roiPts_vec_.end()); + roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end()); #endif } } @@ -640,11 +573,9 @@ void vpMbDepthNormalTracker::segmentPointCloud( continue; for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) { - vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][j], - roiPts_vec[i][j + 1], vpColor::red, 2); + vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2); } - vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], - roiPts_vec[i][roiPts_vec[i].size() - 1], + vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1], vpColor::red, 2); } @@ -653,9 +584,8 @@ void vpMbDepthNormalTracker::segmentPointCloud( } #endif -void vpMbDepthNormalTracker::segmentPointCloud( - const std::vector &point_cloud, const unsigned int width, - const unsigned int height) +void vpMbDepthNormalTracker::segmentPointCloud(const std::vector &point_cloud, const unsigned int width, + const unsigned int height) { m_depthNormalListOfActiveFaces.clear(); m_depthNormalListOfDesiredFeatures.clear(); @@ -663,17 +593,15 @@ void vpMbDepthNormalTracker::segmentPointCloud( #if DEBUG_DISPLAY_DEPTH_NORMAL if (!m_debugDisp_depthNormal->isInitialised()) { m_debugImage_depthNormal.resize(height, width); - m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0, - "Debug display normal depth tracker"); + m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0, "Debug display normal depth tracker"); } m_debugImage_depthNormal = 0; std::vector > roiPts_vec; #endif - for (std::vector::iterator it = - m_depthNormalFaces.begin(); - it != m_depthNormalFaces.end(); ++it) { + for (std::vector::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); + ++it) { vpMbtFaceDepthNormal *face = *it; if (face->isVisible()) { @@ -683,20 +611,18 @@ void vpMbDepthNormalTracker::segmentPointCloud( std::vector > roiPts_vec_; #endif - if (face->computeDesiredFeatures( - cMo, width, height, point_cloud, desired_features, - m_depthNormalSamplingStepX, m_depthNormalSamplingStepY + if (face->computeDesiredFeatures(cMo, width, height, point_cloud, desired_features, m_depthNormalSamplingStepX, + m_depthNormalSamplingStepY #if DEBUG_DISPLAY_DEPTH_NORMAL - , - m_debugImage_depthNormal, roiPts_vec_ + , + m_debugImage_depthNormal, roiPts_vec_ #endif - )) { + )) { m_depthNormalListOfDesiredFeatures.push_back(desired_features); m_depthNormalListOfActiveFaces.push_back(face); #if DEBUG_DISPLAY_DEPTH_NORMAL - roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), - roiPts_vec_.end()); + roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end()); #endif } } @@ -710,11 +636,9 @@ void vpMbDepthNormalTracker::segmentPointCloud( continue; for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) { - vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][j], - roiPts_vec[i][j + 1], vpColor::red, 2); + vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2); } - vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], - roiPts_vec[i][roiPts_vec[i].size() - 1], + vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1], vpColor::red, 2); } @@ -722,23 +646,19 @@ void vpMbDepthNormalTracker::segmentPointCloud( #endif } -void vpMbDepthNormalTracker::setCameraParameters( - const vpCameraParameters &camera) +void vpMbDepthNormalTracker::setCameraParameters(const vpCameraParameters &camera) { this->cam = camera; - for (std::vector::const_iterator it = - m_depthNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); ++it) { (*it)->setCameraParameters(camera); } } -void vpMbDepthNormalTracker::setDepthNormalFaceCentroidMethod( - const vpMbtFaceDepthNormal::vpFaceCentroidType &method) +void vpMbDepthNormalTracker::setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method) { - for (std::vector::const_iterator it = - m_depthNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); ++it) { (*it)->setFaceCentroidMethod(method); } @@ -749,51 +669,43 @@ void vpMbDepthNormalTracker::setDepthNormalFeatureEstimationMethod( { m_depthNormalFeatureEstimationMethod = method; - for (std::vector::const_iterator it = - m_depthNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); ++it) { (*it)->setFeatureEstimationMethod(method); } } -void vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationMethod( - const int method) +void vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationMethod(const int method) { m_depthNormalPclPlaneEstimationMethod = method; - for (std::vector::const_iterator it = - m_depthNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); ++it) { (*it)->setPclPlaneEstimationMethod(method); } } -void vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationRansacMaxIter( - const int maxIter) +void vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter) { m_depthNormalPclPlaneEstimationRansacMaxIter = maxIter; - for (std::vector::const_iterator it = - m_depthNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); ++it) { (*it)->setPclPlaneEstimationRansacMaxIter(maxIter); } } -void vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationRansacThreshold( - const double thresold) +void vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold) { m_depthNormalPclPlaneEstimationRansacThreshold = thresold; - for (std::vector::const_iterator it = - m_depthNormalFaces.begin(); + for (std::vector::const_iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end(); ++it) { (*it)->setPclPlaneEstimationRansacThreshold(thresold); } } -void vpMbDepthNormalTracker::setDepthNormalSamplingStep( - const unsigned int stepX, const unsigned int stepY) +void vpMbDepthNormalTracker::setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY) { if (stepX == 0 || stepY == 0) { std::cerr << "stepX and stepY must be greater than zero!" << std::endl; @@ -810,13 +722,11 @@ void vpMbDepthNormalTracker::setDepthNormalSamplingStep( void vpMbDepthNormalTracker::track(const vpImage &) { - throw vpException(vpException::fatalError, - "Cannot track with a grayscale image!"); + throw vpException(vpException::fatalError, "Cannot track with a grayscale image!"); } #ifdef VISP_HAVE_PCL -void vpMbDepthNormalTracker::track( - const pcl::PointCloud::ConstPtr &point_cloud) +void vpMbDepthNormalTracker::track(const pcl::PointCloud::ConstPtr &point_cloud) { segmentPointCloud(point_cloud); @@ -826,9 +736,8 @@ void vpMbDepthNormalTracker::track( } #endif -void vpMbDepthNormalTracker::track( - const std::vector &point_cloud, const unsigned int width, - const unsigned int height) +void vpMbDepthNormalTracker::track(const std::vector &point_cloud, const unsigned int width, + const unsigned int height) { segmentPointCloud(point_cloud, width, height); @@ -837,35 +746,18 @@ void vpMbDepthNormalTracker::track( computeVisibility(width, height); } -void vpMbDepthNormalTracker::initCircle(const vpPoint & /*p1*/, - const vpPoint & /*p2*/, - const vpPoint & /*p3*/, - const double /*radius*/, - const int /*idFace*/, - const std::string & /*name*/) +void vpMbDepthNormalTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/, + const double /*radius*/, const int /*idFace*/, const std::string & /*name*/) { - throw vpException( - vpException::fatalError, - "vpMbDepthNormalTracker::initCircle() should not be called!"); + throw vpException(vpException::fatalError, "vpMbDepthNormalTracker::initCircle() should not be called!"); } -void vpMbDepthNormalTracker::initCylinder(const vpPoint & /*p1*/, - const vpPoint & /*p2*/, - const double /*radius*/, - const int /*idFace*/, - const std::string & /*name*/) +void vpMbDepthNormalTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const double /*radius*/, + const int /*idFace*/, const std::string & /*name*/) { - throw vpException( - vpException::fatalError, - "vpMbDepthNormalTracker::initCylinder() should not be called!"); + throw vpException(vpException::fatalError, "vpMbDepthNormalTracker::initCylinder() should not be called!"); } -void vpMbDepthNormalTracker::initFaceFromCorners(vpMbtPolygon &polygon) -{ - addFace(polygon, false); -} +void vpMbDepthNormalTracker::initFaceFromCorners(vpMbtPolygon &polygon) { addFace(polygon, false); } -void vpMbDepthNormalTracker::initFaceFromLines(vpMbtPolygon &polygon) -{ - addFace(polygon, true); -} +void vpMbDepthNormalTracker::initFaceFromLines(vpMbtPolygon &polygon) { addFace(polygon, true); } diff --git a/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp b/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp index e004aa9892..f70405ddbb 100644 --- a/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp +++ b/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp @@ -36,8 +36,7 @@ #include #include -#if defined __SSE2__ || defined _M_X64 || \ - (defined _M_IX86_FP && _M_IX86_FP >= 2) +#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2) #include #define VISP_HAVE_SSE2 1 #endif @@ -50,14 +49,11 @@ #endif vpMbtFaceDepthDense::vpMbtFaceDepthDense() - : m_cam(), m_clippingFlag(vpPolygon3D::NO_CLIPPING), m_distFarClip(100), - m_distNearClip(0.001), m_hiddenFace(NULL), m_planeObject(), - m_polygon(NULL), m_useScanLine(false), - m_depthDenseFilteringMethod(DEPTH_OCCUPANCY_RATIO_FILTERING), - m_depthDenseFilteringMaxDist(3.0), m_depthDenseFilteringMinDist(0.8), - m_depthDenseFilteringOccupancyRatio(0.3), m_isTracked(false), - m_isVisible(false), m_listOfFaceLines(), m_planeCamera(), - m_pointCloudFace(), m_polygonLines() + : m_cam(), m_clippingFlag(vpPolygon3D::NO_CLIPPING), m_distFarClip(100), m_distNearClip(0.001), m_hiddenFace(NULL), + m_planeObject(), m_polygon(NULL), m_useScanLine(false), + m_depthDenseFilteringMethod(DEPTH_OCCUPANCY_RATIO_FILTERING), m_depthDenseFilteringMaxDist(3.0), + m_depthDenseFilteringMinDist(0.8), m_depthDenseFilteringOccupancyRatio(0.3), m_isTracked(false), m_isVisible(false), + m_listOfFaceLines(), m_planeCamera(), m_pointCloudFace(), m_polygonLines() { } @@ -81,9 +77,8 @@ vpMbtFaceDepthDense::~vpMbtFaceDepthDense() \param polygon : The index of the polygon to which the line belongs. \param name : the optional name of the line */ -void vpMbtFaceDepthDense::addLine(vpPoint &P1, vpPoint &P2, - vpMbHiddenFaces *const faces, - int polygon, std::string name) +void vpMbtFaceDepthDense::addLine(vpPoint &P1, vpPoint &P2, vpMbHiddenFaces *const faces, int polygon, + std::string name) { // Build a PolygonLine to be able to easily display the lines model PolygonLine polygon_line; @@ -106,12 +101,10 @@ void vpMbtFaceDepthDense::addLine(vpPoint &P1, vpPoint &P2, bool already_here = false; vpMbtDistanceLine *l; - for (std::vector::const_iterator it = - m_listOfFaceLines.begin(); - it != m_listOfFaceLines.end(); ++it) { + for (std::vector::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end(); + ++it) { l = *it; - if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) || - (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) { + 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; @@ -134,12 +127,10 @@ void vpMbtFaceDepthDense::addLine(vpPoint &P1, vpPoint &P2, if (m_clippingFlag != vpPolygon3D::NO_CLIPPING) l->getPolygon().setClipping(m_clippingFlag); - if ((m_clippingFlag & vpPolygon3D::NEAR_CLIPPING) == - vpPolygon3D::NEAR_CLIPPING) + if ((m_clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING) l->getPolygon().setNearClippingDistance(m_distNearClip); - if ((m_clippingFlag & vpPolygon3D::FAR_CLIPPING) == - vpPolygon3D::FAR_CLIPPING) + if ((m_clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING) l->getPolygon().setFarClippingDistance(m_distFarClip); m_listOfFaceLines.push_back(l); @@ -147,14 +138,13 @@ void vpMbtFaceDepthDense::addLine(vpPoint &P1, vpPoint &P2, } #ifdef VISP_HAVE_PCL -bool vpMbtFaceDepthDense::computeDesiredFeatures( - const vpHomogeneousMatrix &cMo, - const pcl::PointCloud::ConstPtr &point_cloud, - const unsigned int stepX, const unsigned int stepY +bool vpMbtFaceDepthDense::computeDesiredFeatures(const vpHomogeneousMatrix &cMo, + const pcl::PointCloud::ConstPtr &point_cloud, + const unsigned int stepX, const unsigned int stepY #if DEBUG_DISPLAY_DEPTH_DENSE - , - vpImage &debugImage, - std::vector > &roiPts_vec + , + vpImage &debugImage, + std::vector > &roiPts_vec #endif ) { @@ -176,16 +166,13 @@ bool vpMbtFaceDepthDense::computeDesiredFeatures( if (roiPts.size() <= 2) { #ifndef NDEBUG - std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" - << std::endl; + std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl; #endif return false; } - if (((m_depthDenseFilteringMethod & MAX_DISTANCE_FILTERING) && - distanceToFace > m_depthDenseFilteringMaxDist) || - ((m_depthDenseFilteringMethod & MIN_DISTANCE_FILTERING) && - distanceToFace < m_depthDenseFilteringMinDist)) { + if (((m_depthDenseFilteringMethod & MAX_DISTANCE_FILTERING) && distanceToFace > m_depthDenseFilteringMaxDist) || + ((m_depthDenseFilteringMethod & MIN_DISTANCE_FILTERING) && distanceToFace < m_depthDenseFilteringMinDist)) { return false; } @@ -193,11 +180,9 @@ bool vpMbtFaceDepthDense::computeDesiredFeatures( vpRect bb = polygon_2d.getBoundingBox(); unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); - unsigned int bottom = - (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); + unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); - unsigned int right = - (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); + unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); bb.setTop(top); bb.setBottom(bottom); @@ -221,20 +206,13 @@ bool vpMbtFaceDepthDense::computeDesiredFeatures( int totalTheoreticalPoints = 0, totalPoints = 0; for (unsigned int i = top; i < bottom; i += stepY) { for (unsigned int j = left; j < right; j += stepX) { - if ((m_useScanLine - ? (i < m_hiddenFace->getMbScanLineRenderer() - .getPrimitiveIDs() - .getHeight() && - j < m_hiddenFace->getMbScanLineRenderer() - .getPrimitiveIDs() - .getWidth() && - m_hiddenFace->getMbScanLineRenderer() - .getPrimitiveIDs()[i][j] == m_polygon->getIndex()) - : polygon_2d.isInside(vpImagePoint(i, j)))) { + if ((m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() && + j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() && + m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex()) + : polygon_2d.isInside(vpImagePoint(i, j)))) { totalTheoreticalPoints++; - if (pcl::isFinite((*point_cloud)(j, i)) && - (*point_cloud)(j, i).z > 0) { + if (pcl::isFinite((*point_cloud)(j, i)) && (*point_cloud)(j, i).z > 0) { totalPoints++; if (checkSSE2) { @@ -278,10 +256,8 @@ bool vpMbtFaceDepthDense::computeDesiredFeatures( } #endif - if (totalPoints == 0 || - ((m_depthDenseFilteringMethod & DEPTH_OCCUPANCY_RATIO_FILTERING) && - totalPoints / (double)totalTheoreticalPoints < - m_depthDenseFilteringOccupancyRatio)) { + if (totalPoints == 0 || ((m_depthDenseFilteringMethod & DEPTH_OCCUPANCY_RATIO_FILTERING) && + totalPoints / (double)totalTheoreticalPoints < m_depthDenseFilteringOccupancyRatio)) { return false; } @@ -289,14 +265,13 @@ bool vpMbtFaceDepthDense::computeDesiredFeatures( } #endif -bool vpMbtFaceDepthDense::computeDesiredFeatures( - const vpHomogeneousMatrix &cMo, const unsigned int width, - const unsigned int height, const std::vector &point_cloud, - const unsigned int stepX, const unsigned int stepY +bool vpMbtFaceDepthDense::computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, + const unsigned int height, const std::vector &point_cloud, + const unsigned int stepX, const unsigned int stepY #if DEBUG_DISPLAY_DEPTH_DENSE - , - vpImage &debugImage, - std::vector > &roiPts_vec + , + vpImage &debugImage, + std::vector > &roiPts_vec #endif ) { @@ -317,16 +292,13 @@ bool vpMbtFaceDepthDense::computeDesiredFeatures( if (roiPts.size() <= 2) { #ifndef NDEBUG - std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" - << std::endl; + std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl; #endif return false; } - if (((m_depthDenseFilteringMethod & MAX_DISTANCE_FILTERING) && - distanceToFace > m_depthDenseFilteringMaxDist) || - ((m_depthDenseFilteringMethod & MIN_DISTANCE_FILTERING) && - distanceToFace < m_depthDenseFilteringMinDist)) { + if (((m_depthDenseFilteringMethod & MAX_DISTANCE_FILTERING) && distanceToFace > m_depthDenseFilteringMaxDist) || + ((m_depthDenseFilteringMethod & MIN_DISTANCE_FILTERING) && distanceToFace < m_depthDenseFilteringMinDist)) { return false; } @@ -334,11 +306,9 @@ bool vpMbtFaceDepthDense::computeDesiredFeatures( vpRect bb = polygon_2d.getBoundingBox(); unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); - unsigned int bottom = - (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); + unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); - unsigned int right = - (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); + unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); bb.setTop(top); bb.setBottom(bottom); @@ -358,16 +328,10 @@ bool vpMbtFaceDepthDense::computeDesiredFeatures( int totalTheoreticalPoints = 0, totalPoints = 0; for (unsigned int i = top; i < bottom; i += stepY) { for (unsigned int j = left; j < right; j += stepX) { - if ((m_useScanLine - ? (i < m_hiddenFace->getMbScanLineRenderer() - .getPrimitiveIDs() - .getHeight() && - j < m_hiddenFace->getMbScanLineRenderer() - .getPrimitiveIDs() - .getWidth() && - m_hiddenFace->getMbScanLineRenderer() - .getPrimitiveIDs()[i][j] == m_polygon->getIndex()) - : polygon_2d.isInside(vpImagePoint(i, j)))) { + if ((m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() && + j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() && + m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex()) + : polygon_2d.isInside(vpImagePoint(i, j)))) { totalTheoreticalPoints++; if (point_cloud[i * width + j][2] > 0) { @@ -414,34 +378,27 @@ bool vpMbtFaceDepthDense::computeDesiredFeatures( } #endif - if (totalPoints == 0 || - ((m_depthDenseFilteringMethod & DEPTH_OCCUPANCY_RATIO_FILTERING) && - totalPoints / (double)totalTheoreticalPoints < - m_depthDenseFilteringOccupancyRatio)) { + if (totalPoints == 0 || ((m_depthDenseFilteringMethod & DEPTH_OCCUPANCY_RATIO_FILTERING) && + totalPoints / (double)totalTheoreticalPoints < m_depthDenseFilteringOccupancyRatio)) { return false; } return true; } -void vpMbtFaceDepthDense::computeVisibility() -{ - m_isVisible = m_polygon->isVisible(); -} +void vpMbtFaceDepthDense::computeVisibility() { m_isVisible = m_polygon->isVisible(); } void vpMbtFaceDepthDense::computeVisibilityDisplay() { // Compute lines visibility, only for display vpMbtDistanceLine *line; - for (std::vector::const_iterator it = - m_listOfFaceLines.begin(); - it != m_listOfFaceLines.end(); ++it) { + for (std::vector::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end(); + ++it) { line = *it; bool isvisible = false; - for (std::list::const_iterator itindex = - line->Lindex_polygon.begin(); - itindex != line->Lindex_polygon.end(); ++itindex) { + for (std::list::const_iterator itindex = line->Lindex_polygon.begin(); itindex != line->Lindex_polygon.end(); + ++itindex) { int index = *itindex; if (index == -1) { isvisible = true; @@ -464,8 +421,8 @@ void vpMbtFaceDepthDense::computeVisibilityDisplay() } } -void vpMbtFaceDepthDense::computeInteractionMatrixAndResidu( - const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error) +void vpMbtFaceDepthDense::computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, + vpColVector &error) { if (m_pointCloudFace.empty()) { L.resize(0, 0); @@ -505,18 +462,14 @@ void vpMbtFaceDepthDense::computeInteractionMatrixAndResidu( double tmp_a1[2], tmp_a2[2], tmp_a3[2]; - for (; cpt <= m_pointCloudFace.size() - 6; - cpt += 6, ptr_point_cloud += 6) { + for (; cpt <= m_pointCloudFace.size() - 6; cpt += 6, ptr_point_cloud += 6) { const __m128d vx = _mm_loadu_pd(ptr_point_cloud); const __m128d vy = _mm_loadu_pd(ptr_point_cloud + 2); const __m128d vz = _mm_loadu_pd(ptr_point_cloud + 4); - const __m128d va1 = - _mm_sub_pd(_mm_mul_pd(vnz, vy), _mm_mul_pd(vny, vz)); - const __m128d va2 = - _mm_sub_pd(_mm_mul_pd(vnx, vz), _mm_mul_pd(vnz, vx)); - const __m128d va3 = - _mm_sub_pd(_mm_mul_pd(vny, vx), _mm_mul_pd(vnx, vy)); + const __m128d va1 = _mm_sub_pd(_mm_mul_pd(vnz, vy), _mm_mul_pd(vny, vz)); + const __m128d va2 = _mm_sub_pd(_mm_mul_pd(vnx, vz), _mm_mul_pd(vnz, vx)); + const __m128d va3 = _mm_sub_pd(_mm_mul_pd(vny, vx), _mm_mul_pd(vnx, vy)); _mm_storeu_pd(tmp_a1, va1); _mm_storeu_pd(tmp_a2, va2); @@ -549,8 +502,7 @@ void vpMbtFaceDepthDense::computeInteractionMatrixAndResidu( ptr_L++; const __m128d verror = - _mm_add_pd(_mm_add_pd(vd, _mm_mul_pd(vnx, vx)), - _mm_add_pd(_mm_mul_pd(vny, vy), _mm_mul_pd(vnz, vz))); + _mm_add_pd(_mm_add_pd(vd, _mm_mul_pd(vnx, vx)), _mm_add_pd(_mm_mul_pd(vny, vy), _mm_mul_pd(vnz, vz))); _mm_storeu_pd(ptr_error, verror); ptr_error += 2; } @@ -621,22 +573,20 @@ void vpMbtFaceDepthDense::computeInteractionMatrixAndResidu( } } -void vpMbtFaceDepthDense::computeROI( - const vpHomogeneousMatrix &cMo, const unsigned int width, - const unsigned int height, std::vector &roiPts +void vpMbtFaceDepthDense::computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width, + const unsigned int height, std::vector &roiPts #if DEBUG_DISPLAY_DEPTH_DENSE - , - std::vector > &roiPts_vec + , + std::vector > &roiPts_vec #endif - , - double &distanceToFace) + , + double &distanceToFace) { if (m_useScanLine || m_clippingFlag > 2) m_cam.computeFov(width, height); if (m_useScanLine) { - for (std::vector::iterator it = m_polygonLines.begin(); - it != m_polygonLines.end(); ++it) { + for (std::vector::iterator it = m_polygonLines.begin(); it != m_polygonLines.end(); ++it) { it->m_p1->changeFrame(cMo); it->m_p2->changeFrame(cMo); @@ -646,29 +596,16 @@ void vpMbtFaceDepthDense::computeROI( it->m_poly.computePolygonClipped(m_cam); if (it->m_poly.polyClipped.size() == 2 && - ((it->m_poly.polyClipped[1].second & - it->m_poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == - 0) && - ((it->m_poly.polyClipped[1].second & - it->m_poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == - 0) && - ((it->m_poly.polyClipped[1].second & - it->m_poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == - 0) && - ((it->m_poly.polyClipped[1].second & - it->m_poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == - 0) && - ((it->m_poly.polyClipped[1].second & - it->m_poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == - 0) && - ((it->m_poly.polyClipped[1].second & - it->m_poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == - 0)) { + ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) && + ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) && + ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) && + ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) && + ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) && + ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) { std::vector > linesLst; - m_hiddenFace->computeScanLineQuery(it->m_poly.polyClipped[0].first, - it->m_poly.polyClipped[1].first, - linesLst, true); + m_hiddenFace->computeScanLineQuery(it->m_poly.polyClipped[0].first, it->m_poly.polyClipped[1].first, linesLst, + true); vpPoint faceCentroid; @@ -676,12 +613,8 @@ void vpMbtFaceDepthDense::computeROI( linesLst[i].first.project(); linesLst[i].second.project(); - vpMeterPixelConversion::convertPoint( - m_cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), - ip1); - vpMeterPixelConversion::convertPoint( - m_cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), - ip2); + vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1); + vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2); it->m_imPt1 = ip1; it->m_imPt2 = ip2; @@ -689,15 +622,9 @@ void vpMbtFaceDepthDense::computeROI( roiPts.push_back(ip1); roiPts.push_back(ip2); - faceCentroid.set_X(faceCentroid.get_X() + - linesLst[i].first.get_X() + - linesLst[i].second.get_X()); - faceCentroid.set_Y(faceCentroid.get_Y() + - linesLst[i].first.get_Y() + - linesLst[i].second.get_Y()); - faceCentroid.set_Z(faceCentroid.get_Z() + - linesLst[i].first.get_Z() + - linesLst[i].second.get_Z()); + faceCentroid.set_X(faceCentroid.get_X() + linesLst[i].first.get_X() + linesLst[i].second.get_X()); + faceCentroid.set_Y(faceCentroid.get_Y() + linesLst[i].first.get_Y() + linesLst[i].second.get_Y()); + faceCentroid.set_Z(faceCentroid.get_Z() + linesLst[i].first.get_Z() + linesLst[i].second.get_Z()); #if DEBUG_DISPLAY_DEPTH_DENSE std::vector roiPts_; @@ -714,9 +641,9 @@ void vpMbtFaceDepthDense::computeROI( faceCentroid.set_Y(faceCentroid.get_Y() / (2 * linesLst.size())); faceCentroid.set_Z(faceCentroid.get_Z() / (2 * linesLst.size())); - distanceToFace = sqrt(faceCentroid.get_X() * faceCentroid.get_X() + - faceCentroid.get_Y() * faceCentroid.get_Y() + - faceCentroid.get_Z() * faceCentroid.get_Z()); + distanceToFace = + sqrt(faceCentroid.get_X() * faceCentroid.get_X() + faceCentroid.get_Y() * faceCentroid.get_Y() + + faceCentroid.get_Z() * faceCentroid.get_Z()); } } } @@ -743,8 +670,7 @@ void vpMbtFaceDepthDense::computeROI( faceCentroid.set_Y(faceCentroid.get_Y() / polygonsClipped.size()); faceCentroid.set_Z(faceCentroid.get_Z() / polygonsClipped.size()); - distanceToFace = sqrt(faceCentroid.get_X() * faceCentroid.get_X() + - faceCentroid.get_Y() * faceCentroid.get_Y() + + distanceToFace = sqrt(faceCentroid.get_X() * faceCentroid.get_X() + faceCentroid.get_Y() * faceCentroid.get_Y() + faceCentroid.get_Z() * faceCentroid.get_Z()); } @@ -754,56 +680,44 @@ void vpMbtFaceDepthDense::computeROI( } } -void vpMbtFaceDepthDense::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const vpColor &col, - const unsigned int thickness, +void vpMbtFaceDepthDense::display(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { if (m_polygon->isVisible() || displayFullModel) { computeVisibilityDisplay(); - for (std::vector::const_iterator it = - m_listOfFaceLines.begin(); - it != m_listOfFaceLines.end(); ++it) { + for (std::vector::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end(); + ++it) { vpMbtDistanceLine *line = *it; line->display(I, cMo, cam, col, thickness, displayFullModel); } } } -void vpMbtFaceDepthDense::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const vpColor &col, - const unsigned int thickness, +void vpMbtFaceDepthDense::display(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { if (m_polygon->isVisible() || displayFullModel) { computeVisibilityDisplay(); - for (std::vector::const_iterator it = - m_listOfFaceLines.begin(); - it != m_listOfFaceLines.end(); ++it) { + for (std::vector::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end(); + ++it) { vpMbtDistanceLine *line = *it; line->display(I, cMo, cam, col, thickness, displayFullModel); } } } -void vpMbtFaceDepthDense::displayFeature(const vpImage & /*I*/, - const vpHomogeneousMatrix & /*cMo*/, - const vpCameraParameters & /*cam*/, - const double /*scale*/, +void vpMbtFaceDepthDense::displayFeature(const vpImage & /*I*/, const vpHomogeneousMatrix & /*cMo*/, + const vpCameraParameters & /*cam*/, const double /*scale*/, const unsigned int /*thickness*/) { } -void vpMbtFaceDepthDense::displayFeature(const vpImage & /*I*/, - const vpHomogeneousMatrix & /*cMo*/, - const vpCameraParameters & /*cam*/, - const double /*scale*/, +void vpMbtFaceDepthDense::displayFeature(const vpImage & /*I*/, const vpHomogeneousMatrix & /*cMo*/, + const vpCameraParameters & /*cam*/, const double /*scale*/, const unsigned int /*thickness*/) { } @@ -817,29 +731,25 @@ void vpMbtFaceDepthDense::displayFeature(const vpImage & /*I*/, \param P1 : The first point to compare \param P2 : The second point to compare */ -bool vpMbtFaceDepthDense::samePoint(const vpPoint &P1, - const vpPoint &P2) const +bool vpMbtFaceDepthDense::samePoint(const vpPoint &P1, const vpPoint &P2) const { 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::epsilon() && - dy <= std::numeric_limits::epsilon() && + if (dx <= std::numeric_limits::epsilon() && dy <= std::numeric_limits::epsilon() && dz <= std::numeric_limits::epsilon()) return true; else return false; } -void vpMbtFaceDepthDense::setCameraParameters( - const vpCameraParameters &camera) +void vpMbtFaceDepthDense::setCameraParameters(const vpCameraParameters &camera) { m_cam = camera; - for (std::vector::const_iterator it = - m_listOfFaceLines.begin(); - it != m_listOfFaceLines.end(); ++it) { + for (std::vector::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end(); + ++it) { (*it)->setCameraParameters(camera); } } @@ -848,9 +758,8 @@ void vpMbtFaceDepthDense::setScanLineVisibilityTest(const bool v) { m_useScanLine = v; - for (std::vector::const_iterator it = - m_listOfFaceLines.begin(); - it != m_listOfFaceLines.end(); ++it) { + for (std::vector::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end(); + ++it) { (*it)->useScanLine = v; } } diff --git a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp index 5f86d598cc..2e7955ec0c 100644 --- a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp +++ b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp @@ -43,8 +43,7 @@ #include #endif -#if defined __SSE2__ || defined _M_X64 || \ - (defined _M_IX86_FP && _M_IX86_FP >= 2) +#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2) #include #define VISP_HAVE_SSE2 1 #endif @@ -57,17 +56,12 @@ #endif vpMbtFaceDepthNormal::vpMbtFaceDepthNormal() - : m_cam(), m_clippingFlag(vpPolygon3D::NO_CLIPPING), m_distFarClip(100), - m_distNearClip(0.001), m_hiddenFace(NULL), m_planeObject(), - m_polygon(NULL), m_useScanLine(false), m_faceActivated(false), - m_faceCentroidMethod(GEOMETRIC_CENTROID), m_faceDesiredCentroid(), - m_faceDesiredNormal(), - m_featureEstimationMethod(ROBUST_FEATURE_ESTIMATION), m_isTracked(false), - m_isVisible(false), m_listOfFaceLines(), m_planeCamera(), - m_pclPlaneEstimationMethod( - 2), // SAC_MSAC, see pcl/sample_consensus/method_types.h - m_pclPlaneEstimationRansacMaxIter(200), - m_pclPlaneEstimationRansacThreshold(0.001), m_polygonLines() + : m_cam(), m_clippingFlag(vpPolygon3D::NO_CLIPPING), m_distFarClip(100), m_distNearClip(0.001), m_hiddenFace(NULL), + m_planeObject(), m_polygon(NULL), m_useScanLine(false), m_faceActivated(false), + m_faceCentroidMethod(GEOMETRIC_CENTROID), m_faceDesiredCentroid(), m_faceDesiredNormal(), + m_featureEstimationMethod(ROBUST_FEATURE_ESTIMATION), m_isTracked(false), m_isVisible(false), m_listOfFaceLines(), + m_planeCamera(), m_pclPlaneEstimationMethod(2), // SAC_MSAC, see pcl/sample_consensus/method_types.h + m_pclPlaneEstimationRansacMaxIter(200), m_pclPlaneEstimationRansacThreshold(0.001), m_polygonLines() { } @@ -91,9 +85,8 @@ vpMbtFaceDepthNormal::~vpMbtFaceDepthNormal() \param polygon : The index of the polygon to which the line belongs. \param name : the optional name of the line */ -void vpMbtFaceDepthNormal::addLine(vpPoint &P1, vpPoint &P2, - vpMbHiddenFaces *const faces, - int polygon, std::string name) +void vpMbtFaceDepthNormal::addLine(vpPoint &P1, vpPoint &P2, vpMbHiddenFaces *const faces, int polygon, + std::string name) { // Build a PolygonLine to be able to easily display the lines model PolygonLine polygon_line; @@ -116,12 +109,10 @@ void vpMbtFaceDepthNormal::addLine(vpPoint &P1, vpPoint &P2, bool already_here = false; vpMbtDistanceLine *l; - for (std::vector::const_iterator it = - m_listOfFaceLines.begin(); - it != m_listOfFaceLines.end(); ++it) { + for (std::vector::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end(); + ++it) { l = *it; - if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) || - (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) { + 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; @@ -144,12 +135,10 @@ void vpMbtFaceDepthNormal::addLine(vpPoint &P1, vpPoint &P2, if (m_clippingFlag != vpPolygon3D::NO_CLIPPING) l->getPolygon().setClipping(m_clippingFlag); - if ((m_clippingFlag & vpPolygon3D::NEAR_CLIPPING) == - vpPolygon3D::NEAR_CLIPPING) + if ((m_clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING) l->getPolygon().setNearClippingDistance(m_distNearClip); - if ((m_clippingFlag & vpPolygon3D::FAR_CLIPPING) == - vpPolygon3D::FAR_CLIPPING) + if ((m_clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING) l->getPolygon().setFarClippingDistance(m_distFarClip); m_listOfFaceLines.push_back(l); @@ -157,16 +146,15 @@ void vpMbtFaceDepthNormal::addLine(vpPoint &P1, vpPoint &P2, } #ifdef VISP_HAVE_PCL -bool vpMbtFaceDepthNormal::computeDesiredFeatures( - const vpHomogeneousMatrix &cMo, const unsigned int width, - const unsigned int height, - const pcl::PointCloud::ConstPtr &point_cloud, - vpColVector &desired_features, const unsigned int stepX, - const unsigned int stepY +bool vpMbtFaceDepthNormal::computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, + const unsigned int height, + const pcl::PointCloud::ConstPtr &point_cloud, + vpColVector &desired_features, const unsigned int stepX, + const unsigned int stepY #if DEBUG_DISPLAY_DEPTH_NORMAL - , - vpImage &debugImage, - std::vector > &roiPts_vec + , + vpImage &debugImage, + std::vector > &roiPts_vec #endif ) { @@ -187,8 +175,7 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( if (roiPts.size() <= 2) { #ifndef NDEBUG - std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" - << std::endl; + std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl; #endif return false; } @@ -197,11 +184,9 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( vpRect bb = polygon_2d.getBoundingBox(); unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); - unsigned int bottom = - (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); + unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); - unsigned int right = - (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); + unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); bb.setTop(top); bb.setBottom(bottom); @@ -209,18 +194,14 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( bb.setRight(right); // Keep only 3D points inside the projected polygon face - pcl::PointCloud::Ptr point_cloud_face( - new pcl::PointCloud); + pcl::PointCloud::Ptr point_cloud_face(new pcl::PointCloud); std::vector point_cloud_face_vec, point_cloud_face_custom; if (m_featureEstimationMethod == ROBUST_FEATURE_ESTIMATION) { - point_cloud_face_custom.reserve( - (size_t)(3 * bb.getWidth() * bb.getHeight())); - point_cloud_face_vec.reserve( - (size_t)(3 * bb.getWidth() * bb.getHeight())); + point_cloud_face_custom.reserve((size_t)(3 * bb.getWidth() * bb.getHeight())); + point_cloud_face_vec.reserve((size_t)(3 * bb.getWidth() * bb.getHeight())); } else if (m_featureEstimationMethod == ROBUST_SVD_PLANE_ESTIMATION) { - point_cloud_face_vec.reserve( - (size_t)(3 * bb.getWidth() * bb.getHeight())); + point_cloud_face_vec.reserve((size_t)(3 * bb.getWidth() * bb.getHeight())); } else if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) { point_cloud_face->reserve((size_t)(bb.getWidth() * bb.getHeight())); } @@ -237,16 +218,10 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( for (unsigned int i = top; i < bottom; i += stepY) { for (unsigned int j = left; j < right; j += stepX) { if (pcl::isFinite((*point_cloud)(j, i)) && (*point_cloud)(j, i).z > 0 && - (m_useScanLine - ? (i < m_hiddenFace->getMbScanLineRenderer() - .getPrimitiveIDs() - .getHeight() && - j < m_hiddenFace->getMbScanLineRenderer() - .getPrimitiveIDs() - .getWidth() && - m_hiddenFace->getMbScanLineRenderer() - .getPrimitiveIDs()[i][j] == m_polygon->getIndex()) - : polygon_2d.isInside(vpImagePoint(i, j)))) { + (m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() && + j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() && + m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex()) + : polygon_2d.isInside(vpImagePoint(i, j)))) { if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) { point_cloud_face->push_back((*point_cloud)(j, i)); @@ -302,8 +277,7 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( } #endif - if (point_cloud_face->empty() && point_cloud_face_custom.empty() && - point_cloud_face_vec.empty()) { + if (point_cloud_face->empty() && point_cloud_face_custom.empty() && point_cloud_face_vec.empty()) { return false; } @@ -311,20 +285,16 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( vpColVector centroid_point(3); if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) { - if (!computeDesiredFeaturesPCL(point_cloud_face, desired_features, - desired_normal, centroid_point)) { + if (!computeDesiredFeaturesPCL(point_cloud_face, desired_features, desired_normal, centroid_point)) { return false; } } else if (m_featureEstimationMethod == ROBUST_SVD_PLANE_ESTIMATION) { - computeDesiredFeaturesSVD(point_cloud_face_vec, cMo, desired_features, - desired_normal, centroid_point); + computeDesiredFeaturesSVD(point_cloud_face_vec, cMo, desired_features, desired_normal, centroid_point); } else if (m_featureEstimationMethod == ROBUST_FEATURE_ESTIMATION) { - computeDesiredFeaturesRobustFeatures( - point_cloud_face_custom, point_cloud_face_vec, cMo, desired_features, - desired_normal, centroid_point); + computeDesiredFeaturesRobustFeatures(point_cloud_face_custom, point_cloud_face_vec, cMo, desired_features, + desired_normal, centroid_point); } else { - throw vpException(vpException::badValue, - "Unknown feature estimation method!"); + throw vpException(vpException::badValue, "Unknown feature estimation method!"); } computeDesiredNormalAndCentroid(cMo, desired_normal, centroid_point); @@ -335,15 +305,15 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( } #endif -bool vpMbtFaceDepthNormal::computeDesiredFeatures( - const vpHomogeneousMatrix &cMo, const unsigned int width, - const unsigned int height, const std::vector &point_cloud, - vpColVector &desired_features, const unsigned int stepX, - const unsigned int stepY +bool vpMbtFaceDepthNormal::computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, + const unsigned int height, + const std::vector &point_cloud, + vpColVector &desired_features, const unsigned int stepX, + const unsigned int stepY #if DEBUG_DISPLAY_DEPTH_NORMAL - , - vpImage &debugImage, - std::vector > &roiPts_vec + , + vpImage &debugImage, + std::vector > &roiPts_vec #endif ) { @@ -364,8 +334,7 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( if (roiPts.size() <= 2) { #ifndef NDEBUG - std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" - << std::endl; + std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl; #endif return false; } @@ -374,11 +343,9 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( vpRect bb = polygon_2d.getBoundingBox(); unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); - unsigned int bottom = - (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); + unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); - unsigned int right = - (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); + unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); bb.setTop(top); bb.setBottom(bottom); @@ -390,8 +357,7 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( point_cloud_face.reserve((size_t)(3 * bb.getWidth() * bb.getHeight())); if (m_featureEstimationMethod == ROBUST_FEATURE_ESTIMATION) { - point_cloud_face_custom.reserve( - (size_t)(3 * bb.getWidth() * bb.getHeight())); + point_cloud_face_custom.reserve((size_t)(3 * bb.getWidth() * bb.getHeight())); } bool checkSSE2 = vpCPUFeatures::checkSSE2(); @@ -406,16 +372,10 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( for (unsigned int i = top; i < bottom; i += stepY) { for (unsigned int j = left; j < right; j += stepX) { if (point_cloud[i * width + j][2] > 0 && - (m_useScanLine - ? (i < m_hiddenFace->getMbScanLineRenderer() - .getPrimitiveIDs() - .getHeight() && - j < m_hiddenFace->getMbScanLineRenderer() - .getPrimitiveIDs() - .getWidth() && - m_hiddenFace->getMbScanLineRenderer() - .getPrimitiveIDs()[i][j] == m_polygon->getIndex()) - : polygon_2d.isInside(vpImagePoint(i, j)))) { + (m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() && + j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() && + m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex()) + : polygon_2d.isInside(vpImagePoint(i, j)))) { // Add point point_cloud_face.push_back(point_cloud[i * width + j][0]); point_cloud_face.push_back(point_cloud[i * width + j][1]); @@ -441,8 +401,7 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( point_cloud_face_custom.push_back(y); point_cloud_face_custom.push_back(prev_z); - point_cloud_face_custom.push_back( - point_cloud[i * width + j][2]); + point_cloud_face_custom.push_back(point_cloud[i * width + j][2]); } #endif } else { @@ -476,30 +435,24 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( #ifdef VISP_HAVE_PCL if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) { - pcl::PointCloud::Ptr point_cloud_face_pcl( - new pcl::PointCloud); + pcl::PointCloud::Ptr point_cloud_face_pcl(new pcl::PointCloud); point_cloud_face_pcl->reserve(point_cloud_face.size() / 3); for (size_t i = 0; i < point_cloud_face.size() / 3; i++) { point_cloud_face_pcl->push_back( - pcl::PointXYZ(point_cloud_face[3 * i], point_cloud_face[3 * i + 1], - point_cloud_face[3 * i + 2])); + pcl::PointXYZ(point_cloud_face[3 * i], point_cloud_face[3 * i + 1], point_cloud_face[3 * i + 2])); } - computeDesiredFeaturesPCL(point_cloud_face_pcl, desired_features, - desired_normal, centroid_point); + computeDesiredFeaturesPCL(point_cloud_face_pcl, desired_features, desired_normal, centroid_point); } else #endif if (m_featureEstimationMethod == ROBUST_SVD_PLANE_ESTIMATION) { - computeDesiredFeaturesSVD(point_cloud_face, cMo, desired_features, - desired_normal, centroid_point); + computeDesiredFeaturesSVD(point_cloud_face, cMo, desired_features, desired_normal, centroid_point); } else if (m_featureEstimationMethod == ROBUST_FEATURE_ESTIMATION) { - computeDesiredFeaturesRobustFeatures( - point_cloud_face_custom, point_cloud_face, cMo, desired_features, - desired_normal, centroid_point); + computeDesiredFeaturesRobustFeatures(point_cloud_face_custom, point_cloud_face, cMo, desired_features, + desired_normal, centroid_point); } else { - throw vpException(vpException::badValue, - "Unknown feature estimation method!"); + throw vpException(vpException::badValue, "Unknown feature estimation method!"); } computeDesiredNormalAndCentroid(cMo, desired_normal, centroid_point); @@ -510,10 +463,9 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures( } #ifdef VISP_HAVE_PCL -bool vpMbtFaceDepthNormal::computeDesiredFeaturesPCL( - const pcl::PointCloud::ConstPtr &point_cloud_face, - vpColVector &desired_features, vpColVector &desired_normal, - vpColVector ¢roid_point) +bool vpMbtFaceDepthNormal::computeDesiredFeaturesPCL(const pcl::PointCloud::ConstPtr &point_cloud_face, + vpColVector &desired_features, vpColVector &desired_normal, + vpColVector ¢roid_point) { try { // Compute plane equation for this subset of point cloud @@ -532,8 +484,7 @@ bool vpMbtFaceDepthNormal::computeDesiredFeaturesPCL( seg.setInputCloud(point_cloud_face); seg.segment(*inliers, *coefficients); - pcl::PointCloud::Ptr point_cloud_face_extracted( - new pcl::PointCloud); + pcl::PointCloud::Ptr point_cloud_face_extracted(new pcl::PointCloud); // Create the filtering object pcl::ExtractIndices extract; @@ -544,20 +495,15 @@ bool vpMbtFaceDepthNormal::computeDesiredFeaturesPCL( extract.filter(*point_cloud_face_extracted); pcl::PointXYZ centroid_point_pcl; - if (pcl::computeCentroid(*point_cloud_face_extracted, - centroid_point_pcl)) { + if (pcl::computeCentroid(*point_cloud_face_extracted, centroid_point_pcl)) { pcl::PointXYZ face_normal; - computeNormalVisibility( - coefficients->values[0], coefficients->values[1], - coefficients->values[2], centroid_point_pcl, face_normal); + computeNormalVisibility(coefficients->values[0], coefficients->values[1], coefficients->values[2], + centroid_point_pcl, face_normal); desired_features.resize(3, false); - desired_features[0] = - -coefficients->values[0] / coefficients->values[3]; - desired_features[1] = - -coefficients->values[1] / coefficients->values[3]; - desired_features[2] = - -coefficients->values[2] / coefficients->values[3]; + desired_features[0] = -coefficients->values[0] / coefficients->values[3]; + desired_features[1] = -coefficients->values[1] / coefficients->values[3]; + desired_features[2] = -coefficients->values[2] / coefficients->values[3]; desired_normal[0] = face_normal.x; desired_normal[1] = face_normal.y; @@ -579,11 +525,12 @@ bool vpMbtFaceDepthNormal::computeDesiredFeaturesPCL( } #endif -void vpMbtFaceDepthNormal::computeDesiredFeaturesRobustFeatures( - const std::vector &point_cloud_face_custom, - const std::vector &point_cloud_face, - const vpHomogeneousMatrix &cMo, vpColVector &desired_features, - vpColVector &desired_normal, vpColVector ¢roid_point) +void vpMbtFaceDepthNormal::computeDesiredFeaturesRobustFeatures(const std::vector &point_cloud_face_custom, + const std::vector &point_cloud_face, + const vpHomogeneousMatrix &cMo, + vpColVector &desired_features, + vpColVector &desired_normal, + vpColVector ¢roid_point) { std::vector weights; double den = 0.0; @@ -602,33 +549,29 @@ void vpMbtFaceDepthNormal::computeDesiredFeaturesRobustFeatures( centroid_point[1] /= den; centroid_point[2] /= den; - computeNormalVisibility(-desired_features[0], -desired_features[1], - -desired_features[2], centroid_point, + computeNormalVisibility(-desired_features[0], -desired_features[1], -desired_features[2], centroid_point, desired_normal); } -void vpMbtFaceDepthNormal::computeDesiredFeaturesSVD( - const std::vector &point_cloud_face, - const vpHomogeneousMatrix &cMo, vpColVector &desired_features, - vpColVector &desired_normal, vpColVector ¢roid_point) +void vpMbtFaceDepthNormal::computeDesiredFeaturesSVD(const std::vector &point_cloud_face, + const vpHomogeneousMatrix &cMo, vpColVector &desired_features, + vpColVector &desired_normal, vpColVector ¢roid_point) { vpColVector plane_equation_SVD; - estimatePlaneEquationSVD(point_cloud_face, cMo, plane_equation_SVD, - centroid_point); + estimatePlaneEquationSVD(point_cloud_face, cMo, plane_equation_SVD, centroid_point); desired_features.resize(3, false); desired_features[0] = -plane_equation_SVD[0] / plane_equation_SVD[3]; desired_features[1] = -plane_equation_SVD[1] / plane_equation_SVD[3]; desired_features[2] = -plane_equation_SVD[2] / plane_equation_SVD[3]; - computeNormalVisibility(-desired_features[0], -desired_features[1], - -desired_features[2], centroid_point, + computeNormalVisibility(-desired_features[0], -desired_features[1], -desired_features[2], centroid_point, desired_normal); } -void vpMbtFaceDepthNormal::computeDesiredNormalAndCentroid( - const vpHomogeneousMatrix &cMo, const vpColVector &desired_normal, - const vpColVector ¢roid_point) +void vpMbtFaceDepthNormal::computeDesiredNormalAndCentroid(const vpHomogeneousMatrix &cMo, + const vpColVector &desired_normal, + const vpColVector ¢roid_point) { // Compute desired centroid in the object frame vpColVector centroid_cam(4); @@ -638,8 +581,7 @@ void vpMbtFaceDepthNormal::computeDesiredNormalAndCentroid( centroid_cam[3] = 1; vpColVector centroid_obj = cMo.inverse() * centroid_cam; - m_faceDesiredCentroid.setWorldCoordinates(centroid_obj[0], centroid_obj[1], - centroid_obj[2]); + m_faceDesiredCentroid.setWorldCoordinates(centroid_obj[0], centroid_obj[1], centroid_obj[2]); // Compute desired face normal in the object frame vpColVector face_normal_cam(4); @@ -649,12 +591,10 @@ void vpMbtFaceDepthNormal::computeDesiredNormalAndCentroid( face_normal_cam[3] = 1; vpColVector face_normal_obj = cMo.inverse() * face_normal_cam; - m_faceDesiredNormal.setWorldCoordinates( - face_normal_obj[0], face_normal_obj[1], face_normal_obj[2]); + m_faceDesiredNormal.setWorldCoordinates(face_normal_obj[0], face_normal_obj[1], face_normal_obj[2]); } -bool vpMbtFaceDepthNormal::computePolygonCentroid( - const std::vector &points_, vpPoint ¢roid) +bool vpMbtFaceDepthNormal::computePolygonCentroid(const std::vector &points_, vpPoint ¢roid) { if (points_.empty()) { return false; @@ -673,23 +613,17 @@ bool vpMbtFaceDepthNormal::computePolygonCentroid( for (size_t i = 0; i < points.size() - 1; i++) { // projection onto xy plane c_x1 += (points[i].get_X() + points[i + 1].get_X()) * - (points[i].get_X() * points[i + 1].get_Y() - - points[i + 1].get_X() * points[i].get_Y()); + (points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y()); c_y += (points[i].get_Y() + points[i + 1].get_Y()) * - (points[i].get_X() * points[i + 1].get_Y() - - points[i + 1].get_X() * points[i].get_Y()); - A1 += points[i].get_X() * points[i + 1].get_Y() - - points[i + 1].get_X() * points[i].get_Y(); + (points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y()); + A1 += points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y(); // projection onto xz plane c_x2 += (points[i].get_X() + points[i + 1].get_X()) * - (points[i].get_X() * points[i + 1].get_Z() - - points[i + 1].get_X() * points[i].get_Z()); + (points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z()); c_z += (points[i].get_Z() + points[i + 1].get_Z()) * - (points[i].get_X() * points[i + 1].get_Z() - - points[i + 1].get_X() * points[i].get_Z()); - A2 += points[i].get_X() * points[i + 1].get_Z() - - points[i + 1].get_X() * points[i].get_Z(); + (points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z()); + A2 += points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z(); } c_x1 /= 3.0 * A1; @@ -709,12 +643,11 @@ bool vpMbtFaceDepthNormal::computePolygonCentroid( return true; } -void vpMbtFaceDepthNormal::computeROI( - const vpHomogeneousMatrix &cMo, const unsigned int width, - const unsigned int height, std::vector &roiPts +void vpMbtFaceDepthNormal::computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width, + const unsigned int height, std::vector &roiPts #if DEBUG_DISPLAY_DEPTH_NORMAL - , - std::vector > &roiPts_vec + , + std::vector > &roiPts_vec #endif ) { @@ -722,8 +655,7 @@ void vpMbtFaceDepthNormal::computeROI( m_cam.computeFov(width, height); if (m_useScanLine) { - for (std::vector::iterator it = m_polygonLines.begin(); - it != m_polygonLines.end(); ++it) { + for (std::vector::iterator it = m_polygonLines.begin(); it != m_polygonLines.end(); ++it) { it->m_p1->changeFrame(cMo); it->m_p2->changeFrame(cMo); @@ -733,40 +665,23 @@ void vpMbtFaceDepthNormal::computeROI( it->m_poly.computePolygonClipped(m_cam); if (it->m_poly.polyClipped.size() == 2 && - ((it->m_poly.polyClipped[1].second & - it->m_poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == - 0) && - ((it->m_poly.polyClipped[1].second & - it->m_poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == - 0) && - ((it->m_poly.polyClipped[1].second & - it->m_poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == - 0) && - ((it->m_poly.polyClipped[1].second & - it->m_poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == - 0) && - ((it->m_poly.polyClipped[1].second & - it->m_poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == - 0) && - ((it->m_poly.polyClipped[1].second & - it->m_poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == - 0)) { + ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) && + ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) && + ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) && + ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) && + ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) && + ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) { std::vector > linesLst; - m_hiddenFace->computeScanLineQuery(it->m_poly.polyClipped[0].first, - it->m_poly.polyClipped[1].first, - linesLst, true); + m_hiddenFace->computeScanLineQuery(it->m_poly.polyClipped[0].first, it->m_poly.polyClipped[1].first, linesLst, + true); for (unsigned int i = 0; i < linesLst.size(); i++) { linesLst[i].first.project(); linesLst[i].second.project(); - vpMeterPixelConversion::convertPoint( - m_cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), - ip1); - vpMeterPixelConversion::convertPoint( - m_cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), - ip2); + vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1); + vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2); it->m_imPt1 = ip1; it->m_imPt2 = ip2; @@ -793,24 +708,19 @@ void vpMbtFaceDepthNormal::computeROI( } } -void vpMbtFaceDepthNormal::computeVisibility() -{ - m_isVisible = m_polygon->isVisible(); -} +void vpMbtFaceDepthNormal::computeVisibility() { m_isVisible = m_polygon->isVisible(); } void vpMbtFaceDepthNormal::computeVisibilityDisplay() { // Compute lines visibility, only for display vpMbtDistanceLine *line; - for (std::vector::const_iterator it = - m_listOfFaceLines.begin(); - it != m_listOfFaceLines.end(); ++it) { + for (std::vector::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end(); + ++it) { line = *it; bool isvisible = false; - for (std::list::const_iterator itindex = - line->Lindex_polygon.begin(); - itindex != line->Lindex_polygon.end(); ++itindex) { + for (std::list::const_iterator itindex = line->Lindex_polygon.begin(); itindex != line->Lindex_polygon.end(); + ++itindex) { int index = *itindex; if (index == -1) { isvisible = true; @@ -833,10 +743,9 @@ void vpMbtFaceDepthNormal::computeVisibilityDisplay() } } -void vpMbtFaceDepthNormal::computeNormalVisibility( - const double nx, const double ny, const double nz, - const vpHomogeneousMatrix &cMo, const vpCameraParameters &camera, - vpColVector &correct_normal, vpPoint ¢roid) +void vpMbtFaceDepthNormal::computeNormalVisibility(const double nx, const double ny, const double nz, + const vpHomogeneousMatrix &cMo, const vpCameraParameters &camera, + vpColVector &correct_normal, vpPoint ¢roid) { vpColVector faceNormal(3); faceNormal[0] = nx; @@ -897,9 +806,8 @@ void vpMbtFaceDepthNormal::computeNormalVisibility( } #ifdef VISP_HAVE_PCL -void vpMbtFaceDepthNormal::computeNormalVisibility( - const float nx, const float ny, const float nz, - const pcl::PointXYZ ¢roid_point, pcl::PointXYZ &face_normal) +void vpMbtFaceDepthNormal::computeNormalVisibility(const float nx, const float ny, const float nz, + const pcl::PointXYZ ¢roid_point, pcl::PointXYZ &face_normal) { vpColVector faceNormal(3); faceNormal[0] = nx; @@ -917,15 +825,13 @@ void vpMbtFaceDepthNormal::computeNormalVisibility( if (angle < M_PI_2) { face_normal = pcl::PointXYZ(faceNormal[0], faceNormal[1], faceNormal[2]); } else { - face_normal = - pcl::PointXYZ(-faceNormal[0], -faceNormal[1], -faceNormal[2]); + face_normal = pcl::PointXYZ(-faceNormal[0], -faceNormal[1], -faceNormal[2]); } } #endif -void vpMbtFaceDepthNormal::computeNormalVisibility( - const double nx, const double ny, const double nz, - const vpColVector ¢roid_point, vpColVector &face_normal) +void vpMbtFaceDepthNormal::computeNormalVisibility(const double nx, const double ny, const double nz, + const vpColVector ¢roid_point, vpColVector &face_normal) { face_normal.resize(3, false); face_normal[0] = nx; @@ -944,8 +850,7 @@ void vpMbtFaceDepthNormal::computeNormalVisibility( } } -void vpMbtFaceDepthNormal::computeInteractionMatrix( - const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features) +void vpMbtFaceDepthNormal::computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features) { L.resize(3, 6, false, false); @@ -990,48 +895,38 @@ void vpMbtFaceDepthNormal::computeInteractionMatrix( L[2][5] = 0.0; } -void vpMbtFaceDepthNormal::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const vpColor &col, - const unsigned int thickness, +void vpMbtFaceDepthNormal::display(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { if (m_polygon->isVisible() || displayFullModel) { computeVisibilityDisplay(); - for (std::vector::const_iterator it = - m_listOfFaceLines.begin(); - it != m_listOfFaceLines.end(); ++it) { + for (std::vector::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end(); + ++it) { vpMbtDistanceLine *line = *it; line->display(I, cMo, cam, col, thickness, displayFullModel); } } } -void vpMbtFaceDepthNormal::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const vpColor &col, - const unsigned int thickness, +void vpMbtFaceDepthNormal::display(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { if (m_polygon->isVisible() || displayFullModel) { computeVisibilityDisplay(); - for (std::vector::const_iterator it = - m_listOfFaceLines.begin(); - it != m_listOfFaceLines.end(); ++it) { + for (std::vector::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end(); + ++it) { vpMbtDistanceLine *line = *it; line->display(I, cMo, cam, col, thickness, displayFullModel); } } } -void vpMbtFaceDepthNormal::displayFeature(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const double scale, +void vpMbtFaceDepthNormal::displayFeature(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &cam, const double scale, const unsigned int thickness) { if (m_faceActivated /*&& m_isTracked*/ && m_isVisible) { @@ -1041,8 +936,7 @@ void vpMbtFaceDepthNormal::displayFeature(const vpImage &I, pt_centroid.project(); vpImagePoint im_centroid; - vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), - pt_centroid.get_y(), im_centroid); + vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid); vpPoint pt_normal = m_faceDesiredNormal; pt_normal.changeFrame(cMo); @@ -1055,11 +949,9 @@ void vpMbtFaceDepthNormal::displayFeature(const vpImage &I, pt_extremity.project(); vpImagePoint im_extremity; - vpMeterPixelConversion::convertPoint(cam, pt_extremity.get_x(), - pt_extremity.get_y(), im_extremity); + vpMeterPixelConversion::convertPoint(cam, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity); - vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::blue, 4, 2, - thickness); + vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::blue, 4, 2, thickness); // Current feature // Transform the plane equation for the current pose @@ -1073,30 +965,24 @@ void vpMbtFaceDepthNormal::displayFeature(const vpImage &I, vpColVector correct_normal; vpCameraParameters cam_copy = cam; cam_copy.computeFov(I.getWidth(), I.getHeight()); - computeNormalVisibility(ux, uy, uz, cMo, cam_copy, correct_normal, - pt_centroid); + computeNormalVisibility(ux, uy, uz, cMo, cam_copy, correct_normal, pt_centroid); pt_centroid.project(); - vpMeterPixelConversion::convertPoint(cam_copy, pt_centroid.get_x(), - pt_centroid.get_y(), im_centroid); + vpMeterPixelConversion::convertPoint(cam_copy, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid); pt_extremity.set_X(pt_centroid.get_X() + correct_normal[0] * scale); pt_extremity.set_Y(pt_centroid.get_Y() + correct_normal[1] * scale); pt_extremity.set_Z(pt_centroid.get_Z() + correct_normal[2] * scale); pt_extremity.project(); - vpMeterPixelConversion::convertPoint(cam_copy, pt_extremity.get_x(), - pt_extremity.get_y(), im_extremity); + vpMeterPixelConversion::convertPoint(cam_copy, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity); - vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::red, 4, 2, - thickness); + vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::red, 4, 2, thickness); } } -void vpMbtFaceDepthNormal::displayFeature(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &cam, - const double scale, +void vpMbtFaceDepthNormal::displayFeature(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &cam, const double scale, const unsigned int thickness) { if (m_faceActivated /*&& m_isTracked*/ && m_isVisible) { @@ -1106,8 +992,7 @@ void vpMbtFaceDepthNormal::displayFeature(const vpImage &I, pt_centroid.project(); vpImagePoint im_centroid; - vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), - pt_centroid.get_y(), im_centroid); + vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid); vpPoint pt_normal = m_faceDesiredNormal; pt_normal.changeFrame(cMo); @@ -1120,11 +1005,9 @@ void vpMbtFaceDepthNormal::displayFeature(const vpImage &I, pt_extremity.project(); vpImagePoint im_extremity; - vpMeterPixelConversion::convertPoint(cam, pt_extremity.get_x(), - pt_extremity.get_y(), im_extremity); + vpMeterPixelConversion::convertPoint(cam, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity); - vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::blue, 4, 2, - thickness); + vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::blue, 4, 2, thickness); // Current feature // Transform the plane equation for the current pose @@ -1138,30 +1021,24 @@ void vpMbtFaceDepthNormal::displayFeature(const vpImage &I, vpColVector correct_normal; vpCameraParameters cam_copy = cam; cam_copy.computeFov(I.getWidth(), I.getHeight()); - computeNormalVisibility(ux, uy, uz, cMo, cam_copy, correct_normal, - pt_centroid); + computeNormalVisibility(ux, uy, uz, cMo, cam_copy, correct_normal, pt_centroid); pt_centroid.project(); - vpMeterPixelConversion::convertPoint(cam_copy, pt_centroid.get_x(), - pt_centroid.get_y(), im_centroid); + vpMeterPixelConversion::convertPoint(cam_copy, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid); pt_extremity.set_X(pt_centroid.get_X() + correct_normal[0] * scale); pt_extremity.set_Y(pt_centroid.get_Y() + correct_normal[1] * scale); pt_extremity.set_Z(pt_centroid.get_Z() + correct_normal[2] * scale); pt_extremity.project(); - vpMeterPixelConversion::convertPoint(cam_copy, pt_extremity.get_x(), - pt_extremity.get_y(), im_extremity); + vpMeterPixelConversion::convertPoint(cam_copy, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity); - vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::red, 4, 2, - thickness); + vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::red, 4, 2, thickness); } } -void vpMbtFaceDepthNormal::estimateFeatures( - const std::vector &point_cloud_face, - const vpHomogeneousMatrix &cMo, vpColVector &x_estimated, - std::vector &w) +void vpMbtFaceDepthNormal::estimateFeatures(const std::vector &point_cloud_face, const vpHomogeneousMatrix &cMo, + vpColVector &x_estimated, std::vector &w) { vpMbtTukeyEstimator tukey_robust; std::vector residues(point_cloud_face.size() / 3); @@ -1207,16 +1084,14 @@ void vpMbtFaceDepthNormal::estimateFeatures( double *ptr_residues = &residues[0]; - for (; cpt <= point_cloud_face.size() - 6; - cpt += 6, ptr_point_cloud += 6, ptr_residues += 2) { + for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_residues += 2) { const __m128d vxi = _mm_loadu_pd(ptr_point_cloud); const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2); const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4); const __m128d vinvZi = _mm_div_pd(vones, vZi); - const __m128d tmp = _mm_add_pd( - _mm_add_pd(_mm_mul_pd(vA, vxi), _mm_mul_pd(vB, vyi)), - _mm_sub_pd(vC, vinvZi)); + const __m128d tmp = + _mm_add_pd(_mm_add_pd(_mm_mul_pd(vA, vxi), _mm_mul_pd(vB, vyi)), _mm_sub_pd(vC, vinvZi)); _mm_storeu_pd(ptr_residues, tmp); } } @@ -1251,31 +1126,24 @@ void vpMbtFaceDepthNormal::estimateFeatures( const __m128d vones = _mm_set1_pd(1.0); - for (; cpt <= point_cloud_face.size() - 6; - cpt += 6, ptr_point_cloud += 6, ptr_w += 2) { - const __m128d vwi2 = - _mm_mul_pd(_mm_loadu_pd(ptr_w), _mm_loadu_pd(ptr_w)); + for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_w += 2) { + const __m128d vwi2 = _mm_mul_pd(_mm_loadu_pd(ptr_w), _mm_loadu_pd(ptr_w)); const __m128d vxi = _mm_loadu_pd(ptr_point_cloud); const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2); const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4); const __m128d vinvZi = _mm_div_pd(vones, vZi); - vsum_wi2_xi2 = _mm_add_pd(vsum_wi2_xi2, - _mm_mul_pd(vwi2, _mm_mul_pd(vxi, vxi))); - vsum_wi2_yi2 = _mm_add_pd(vsum_wi2_yi2, - _mm_mul_pd(vwi2, _mm_mul_pd(vyi, vyi))); + vsum_wi2_xi2 = _mm_add_pd(vsum_wi2_xi2, _mm_mul_pd(vwi2, _mm_mul_pd(vxi, vxi))); + vsum_wi2_yi2 = _mm_add_pd(vsum_wi2_yi2, _mm_mul_pd(vwi2, _mm_mul_pd(vyi, vyi))); vsum_wi2 = _mm_add_pd(vsum_wi2, vwi2); - vsum_wi2_xi_yi = _mm_add_pd(vsum_wi2_xi_yi, - _mm_mul_pd(vwi2, _mm_mul_pd(vxi, vyi))); + vsum_wi2_xi_yi = _mm_add_pd(vsum_wi2_xi_yi, _mm_mul_pd(vwi2, _mm_mul_pd(vxi, vyi))); vsum_wi2_xi = _mm_add_pd(vsum_wi2_xi, _mm_mul_pd(vwi2, vxi)); vsum_wi2_yi = _mm_add_pd(vsum_wi2_yi, _mm_mul_pd(vwi2, vyi)); const __m128d vwi2_invZi = _mm_mul_pd(vwi2, vinvZi); - vsum_wi2_xi_Zi = - _mm_add_pd(vsum_wi2_xi_Zi, _mm_mul_pd(vxi, vwi2_invZi)); - vsum_wi2_yi_Zi = - _mm_add_pd(vsum_wi2_yi_Zi, _mm_mul_pd(vyi, vwi2_invZi)); + vsum_wi2_xi_Zi = _mm_add_pd(vsum_wi2_xi_Zi, _mm_mul_pd(vxi, vwi2_invZi)); + vsum_wi2_yi_Zi = _mm_add_pd(vsum_wi2_yi_Zi, _mm_mul_pd(vyi, vwi2_invZi)); vsum_wi2_Zi = _mm_add_pd(vsum_wi2_Zi, vwi2_invZi); } } @@ -1340,12 +1208,9 @@ void vpMbtFaceDepthNormal::estimateFeatures( Mat33 minv = ATA_3x3.inverse(); - A = minv[0] * sum_wi2_xi_Zi + minv[1] * sum_wi2_yi_Zi + - minv[2] * sum_wi2_Zi; - B = minv[3] * sum_wi2_xi_Zi + minv[4] * sum_wi2_yi_Zi + - minv[5] * sum_wi2_Zi; - C = minv[6] * sum_wi2_xi_Zi + minv[7] * sum_wi2_yi_Zi + - minv[8] * sum_wi2_Zi; + A = minv[0] * sum_wi2_xi_Zi + minv[1] * sum_wi2_yi_Zi + minv[2] * sum_wi2_Zi; + B = minv[3] * sum_wi2_xi_Zi + minv[4] * sum_wi2_yi_Zi + minv[5] * sum_wi2_Zi; + C = minv[6] * sum_wi2_xi_Zi + minv[7] * sum_wi2_yi_Zi + minv[8] * sum_wi2_Zi; cpt = 0; @@ -1363,16 +1228,13 @@ void vpMbtFaceDepthNormal::estimateFeatures( double *ptr_residues = &residues[0]; - for (; cpt <= point_cloud_face.size() - 6; - cpt += 6, ptr_point_cloud += 6, ptr_residues += 2) { + for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_residues += 2) { const __m128d vxi = _mm_loadu_pd(ptr_point_cloud); const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2); const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4); const __m128d vinvZi = _mm_div_pd(vones, vZi); - const __m128d tmp = - _mm_add_pd(_mm_add_pd(_mm_mul_pd(vA, vxi), _mm_mul_pd(vB, vyi)), - _mm_sub_pd(vC, vinvZi)); + const __m128d tmp = _mm_add_pd(_mm_add_pd(_mm_mul_pd(vA, vxi), _mm_mul_pd(vB, vyi)), _mm_sub_pd(vC, vinvZi)); verror = _mm_add_pd(verror, _mm_mul_pd(tmp, tmp)); _mm_storeu_pd(ptr_residues, tmp); @@ -1462,12 +1324,9 @@ void vpMbtFaceDepthNormal::estimateFeatures( Mat33 minv = ATA_3x3.inverse(); - A = minv[0] * sum_wi2_xi_Zi + minv[1] * sum_wi2_yi_Zi + - minv[2] * sum_wi2_Zi; - B = minv[3] * sum_wi2_xi_Zi + minv[4] * sum_wi2_yi_Zi + - minv[5] * sum_wi2_Zi; - C = minv[6] * sum_wi2_xi_Zi + minv[7] * sum_wi2_yi_Zi + - minv[8] * sum_wi2_Zi; + A = minv[0] * sum_wi2_xi_Zi + minv[1] * sum_wi2_yi_Zi + minv[2] * sum_wi2_Zi; + B = minv[3] * sum_wi2_xi_Zi + minv[4] * sum_wi2_yi_Zi + minv[5] * sum_wi2_Zi; + C = minv[6] * sum_wi2_xi_Zi + minv[7] * sum_wi2_yi_Zi + minv[8] * sum_wi2_Zi; prev_error = error; error = 0.0; @@ -1494,10 +1353,9 @@ void vpMbtFaceDepthNormal::estimateFeatures( x_estimated[2] = C; } -void vpMbtFaceDepthNormal::estimatePlaneEquationSVD( - const std::vector &point_cloud_face, - const vpHomogeneousMatrix &cMo, vpColVector &plane_equation_estimated, - vpColVector ¢roid) +void vpMbtFaceDepthNormal::estimatePlaneEquationSVD(const std::vector &point_cloud_face, + const vpHomogeneousMatrix &cMo, + vpColVector &plane_equation_estimated, vpColVector ¢roid) { const unsigned int max_iter = 10; double prev_error = 1e3; @@ -1509,8 +1367,7 @@ void vpMbtFaceDepthNormal::estimatePlaneEquationSVD( vpMbtTukeyEstimator tukey; vpColVector normal; - for (unsigned int iter = 0; - iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) { + for (unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) { if (iter != 0) { tukey.MEstimator(residues, weights, 1e-4); } else { @@ -1525,8 +1382,7 @@ void vpMbtFaceDepthNormal::estimatePlaneEquationSVD( // Compute distance point to estimated plane for (size_t i = 0; i < point_cloud_face.size() / 3; i++) { - residues[i] = std::fabs(A * point_cloud_face[3 * i] + - B * point_cloud_face[3 * i + 1] + + residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] + C * point_cloud_face[3 * i + 2] + D) / sqrt(A * A + B * B + C * C); } @@ -1552,12 +1408,9 @@ void vpMbtFaceDepthNormal::estimatePlaneEquationSVD( // Minimization for (size_t i = 0; i < point_cloud_face.size() / 3; i++) { - M[(unsigned int)i][0] = - weights[i] * (point_cloud_face[3 * i] - centroid_x); - M[(unsigned int)i][1] = - weights[i] * (point_cloud_face[3 * i + 1] - centroid_y); - M[(unsigned int)i][2] = - weights[i] * (point_cloud_face[3 * i + 2] - centroid_z); + M[(unsigned int)i][0] = weights[i] * (point_cloud_face[3 * i] - centroid_x); + M[(unsigned int)i][1] = weights[i] * (point_cloud_face[3 * i + 1] - centroid_y); + M[(unsigned int)i][2] = weights[i] * (point_cloud_face[3 * i + 2] - centroid_z); } vpMatrix J = M.t() * M; @@ -1591,8 +1444,7 @@ void vpMbtFaceDepthNormal::estimatePlaneEquationSVD( prev_error = error; error = 0.0; for (size_t i = 0; i < point_cloud_face.size() / 3; i++) { - residues[i] = std::fabs(A * point_cloud_face[3 * i] + - B * point_cloud_face[3 * i + 1] + + residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] + C * point_cloud_face[3 * i + 2] + D) / sqrt(A * A + B * B + C * C); error += residues[i] * residues[i]; @@ -1638,29 +1490,25 @@ void vpMbtFaceDepthNormal::estimatePlaneEquationSVD( \param P1 : The first point to compare \param P2 : The second point to compare */ -bool vpMbtFaceDepthNormal::samePoint(const vpPoint &P1, - const vpPoint &P2) const +bool vpMbtFaceDepthNormal::samePoint(const vpPoint &P1, const vpPoint &P2) const { 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::epsilon() && - dy <= std::numeric_limits::epsilon() && + if (dx <= std::numeric_limits::epsilon() && dy <= std::numeric_limits::epsilon() && dz <= std::numeric_limits::epsilon()) return true; else return false; } -void vpMbtFaceDepthNormal::setCameraParameters( - const vpCameraParameters &camera) +void vpMbtFaceDepthNormal::setCameraParameters(const vpCameraParameters &camera) { m_cam = camera; - for (std::vector::const_iterator it = - m_listOfFaceLines.begin(); - it != m_listOfFaceLines.end(); ++it) { + for (std::vector::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end(); + ++it) { (*it)->setCameraParameters(camera); } } @@ -1669,9 +1517,8 @@ void vpMbtFaceDepthNormal::setScanLineVisibilityTest(const bool v) { m_useScanLine = v; - for (std::vector::const_iterator it = - m_listOfFaceLines.begin(); - it != m_listOfFaceLines.end(); ++it) { + for (std::vector::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end(); + ++it) { (*it)->useScanLine = v; } } diff --git a/modules/tracker/mbt/src/depth/vpMbtTukeyEstimator.cpp b/modules/tracker/mbt/src/depth/vpMbtTukeyEstimator.cpp index 4fa9393f85..00143d5bd6 100644 --- a/modules/tracker/mbt/src/depth/vpMbtTukeyEstimator.cpp +++ b/modules/tracker/mbt/src/depth/vpMbtTukeyEstimator.cpp @@ -50,8 +50,7 @@ #include #endif -#if defined __SSE2__ || defined _M_X64 || \ - (defined _M_IX86_FP && _M_IX86_FP >= 2) +#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2) #include #define VISP_HAVE_SSE2 1 @@ -107,8 +106,7 @@ template T vpMbtTukeyEstimator::getMedian(std::vector &vec) // std::allocator > const&, std::vector // >&, double)' template -void vpMbtTukeyEstimator::MEstimator_impl(const std::vector &residues, - std::vector &weights, +void vpMbtTukeyEstimator::MEstimator_impl(const std::vector &residues, std::vector &weights, const T NoiseThreshold) { if (residues.empty()) { @@ -145,9 +143,8 @@ void vpMbtTukeyEstimator::MEstimator_impl(const std::vector &residues, } template <> -void vpMbtTukeyEstimator::MEstimator_impl_ssse3( - const std::vector &residues, std::vector &weights, - const float NoiseThreshold) +void vpMbtTukeyEstimator::MEstimator_impl_ssse3(const std::vector &residues, std::vector &weights, + const float NoiseThreshold) { #if VISP_HAVE_SSSE3 if (residues.empty()) { @@ -165,8 +162,7 @@ void vpMbtTukeyEstimator::MEstimator_impl_ssse3( if (m_residues.size() >= 4) { for (i = 0; i <= m_residues.size() - 4; i += 4) { __m128 residues_128 = _mm_loadu_ps(residues.data() + i); - _mm_storeu_ps(m_normres.data() + i, - abs_ps(_mm_sub_ps(residues_128, med_128))); + _mm_storeu_ps(m_normres.data() + i, abs_ps(_mm_sub_ps(residues_128, med_128))); } } @@ -195,9 +191,8 @@ void vpMbtTukeyEstimator::MEstimator_impl_ssse3( } template <> -void vpMbtTukeyEstimator::MEstimator_impl_ssse3( - const std::vector &residues, std::vector &weights, - const double NoiseThreshold) +void vpMbtTukeyEstimator::MEstimator_impl_ssse3(const std::vector &residues, + std::vector &weights, const double NoiseThreshold) { #if VISP_HAVE_SSSE3 if (residues.empty()) { @@ -239,9 +234,8 @@ void vpMbtTukeyEstimator::MEstimator_impl_ssse3( } template <> -void vpMbtTukeyEstimator::MEstimator( - const std::vector &residues, std::vector &weights, - const float NoiseThreshold) +void vpMbtTukeyEstimator::MEstimator(const std::vector &residues, std::vector &weights, + const float NoiseThreshold) { bool checkSSSE3 = vpCPUFeatures::checkSSSE3(); #if !VISP_HAVE_SSSE3 @@ -255,9 +249,8 @@ void vpMbtTukeyEstimator::MEstimator( } template <> -void vpMbtTukeyEstimator::MEstimator( - const std::vector &residues, std::vector &weights, - const double NoiseThreshold) +void vpMbtTukeyEstimator::MEstimator(const std::vector &residues, std::vector &weights, + const double NoiseThreshold) { bool checkSSSE3 = vpCPUFeatures::checkSSSE3(); #if !VISP_HAVE_SSSE3 @@ -270,9 +263,7 @@ void vpMbtTukeyEstimator::MEstimator( MEstimator_impl(residues, weights, NoiseThreshold); } -template -void vpMbtTukeyEstimator::psiTukey(const T sig, std::vector &x, - vpColVector &weights) +template void vpMbtTukeyEstimator::psiTukey(const T sig, std::vector &x, vpColVector &weights) { T cst_const = static_cast(4.6851); T inv_cst_const = 1 / cst_const; @@ -281,10 +272,10 @@ void vpMbtTukeyEstimator::psiTukey(const T sig, std::vector &x, for (unsigned int i = 0; i < (unsigned int)x.size(); i++) { #if USE_ORIGINAL_TUKEY_CODE if (std::fabs(sig) <= std::numeric_limits::epsilon() && - std::fabs(weights[i]) > - std::numeric_limits::epsilon() // sig should be equal to 0 - // only if NoiseThreshold == - // 0 + std::fabs(weights[i]) > std::numeric_limits::epsilon() // sig should be equal to 0 + // only if NoiseThreshold + // == + // 0 ) { weights[i] = 1; continue; @@ -295,16 +286,14 @@ void vpMbtTukeyEstimator::psiTukey(const T sig, std::vector &x, if ((std::fabs(xi_sig) <= cst_const) #if USE_ORIGINAL_TUKEY_CODE - && std::fabs(weights[i]) > - std::numeric_limits::epsilon() // Consider the previous - // weights here + && std::fabs(weights[i]) > std::numeric_limits::epsilon() // Consider the previous + // weights here #endif ) { weights[i] = (1 - (xi_sig * inv_cst_const) * (xi_sig * inv_cst_const)) * - (1 - (xi_sig * inv_cst_const) * - (xi_sig * inv_cst_const)); // vpMath::sqr( 1 - - // vpMath::sqr(xi_sig * - // inv_cst_const) ); + (1 - (xi_sig * inv_cst_const) * (xi_sig * inv_cst_const)); // vpMath::sqr( 1 - + // vpMath::sqr(xi_sig * + // inv_cst_const) ); } else { // Outlier - could resize list of points tracked here? weights[i] = 0; @@ -313,8 +302,7 @@ void vpMbtTukeyEstimator::psiTukey(const T sig, std::vector &x, } template <> -void vpMbtTukeyEstimator::MEstimator(const vpColVector &residues, - vpColVector &weights, +void vpMbtTukeyEstimator::MEstimator(const vpColVector &residues, vpColVector &weights, const double NoiseThreshold) { if (residues.size() == 0) { @@ -323,8 +311,7 @@ void vpMbtTukeyEstimator::MEstimator(const vpColVector &residues, m_residues.resize(0); m_residues.reserve(residues.size()); - m_residues.insert(m_residues.end(), &residues.data[0], - &residues.data[residues.size()]); + m_residues.insert(m_residues.end(), &residues.data[0], &residues.data[residues.size()]); double med = getMedian(m_residues); @@ -349,8 +336,7 @@ void vpMbtTukeyEstimator::MEstimator(const vpColVector &residues, } template <> -void vpMbtTukeyEstimator::MEstimator(const vpColVector &residues, - vpColVector &weights, +void vpMbtTukeyEstimator::MEstimator(const vpColVector &residues, vpColVector &weights, const double NoiseThreshold) { if (residues.size() == 0) { @@ -385,9 +371,7 @@ void vpMbtTukeyEstimator::MEstimator(const vpColVector &residues, psiTukey(sigma, m_normres, weights); } -template -void vpMbtTukeyEstimator::psiTukey(const T sig, std::vector &x, - std::vector &weights) +template void vpMbtTukeyEstimator::psiTukey(const T sig, std::vector &x, std::vector &weights) { T cst_const = static_cast(4.6851); T inv_cst_const = 1 / cst_const; @@ -396,9 +380,8 @@ void vpMbtTukeyEstimator::psiTukey(const T sig, std::vector &x, for (size_t i = 0; i < x.size(); i++) { #if USE_ORIGINAL_TUKEY_CODE if (std::fabs(sig) <= std::numeric_limits::epsilon() && - std::fabs(weights[i]) > - std::numeric_limits::epsilon() // sig should be equal to 0 only - // if NoiseThreshold == 0 + std::fabs(weights[i]) > std::numeric_limits::epsilon() // sig should be equal to 0 only + // if NoiseThreshold == 0 ) { weights[i] = 1; continue; @@ -409,9 +392,8 @@ void vpMbtTukeyEstimator::psiTukey(const T sig, std::vector &x, if ((std::fabs(xi_sig) <= cst_const) #if USE_ORIGINAL_TUKEY_CODE - && std::fabs(weights[i]) > - std::numeric_limits::epsilon() // Consider the previous - // weights here + && std::fabs(weights[i]) > std::numeric_limits::epsilon() // Consider the previous + // weights here #endif ) { // vpMath::sqr( 1 - vpMath::sqr(xi_sig * inv_cst_const) ); diff --git a/modules/tracker/mbt/src/edge/vpMbEdgeMultiTracker.cpp b/modules/tracker/mbt/src/edge/vpMbEdgeMultiTracker.cpp index e5a09c52d5..072c667511 100644 --- a/modules/tracker/mbt/src/edge/vpMbEdgeMultiTracker.cpp +++ b/modules/tracker/mbt/src/edge/vpMbEdgeMultiTracker.cpp @@ -53,10 +53,8 @@ Basic constructor */ vpMbEdgeMultiTracker::vpMbEdgeMultiTracker() - : m_mapOfCameraTransformationMatrix(), m_mapOfEdgeTrackers(), - m_mapOfPyramidalImages(), m_referenceCameraName("Camera"), - m_L_edgeMulti(), m_error_edgeMulti(), m_w_edgeMulti(), - m_weightedError_edgeMulti() + : m_mapOfCameraTransformationMatrix(), m_mapOfEdgeTrackers(), m_mapOfPyramidalImages(), + m_referenceCameraName("Camera"), m_L_edgeMulti(), m_error_edgeMulti(), m_w_edgeMulti(), m_weightedError_edgeMulti() { m_mapOfEdgeTrackers["Camera"] = new vpMbEdgeTracker(); @@ -70,16 +68,12 @@ vpMbEdgeMultiTracker::vpMbEdgeMultiTracker() \param nbCameras : Number of cameras to use. */ vpMbEdgeMultiTracker::vpMbEdgeMultiTracker(const unsigned int nbCameras) - : m_mapOfCameraTransformationMatrix(), m_mapOfEdgeTrackers(), - m_mapOfPyramidalImages(), m_referenceCameraName("Camera"), - m_L_edgeMulti(), m_error_edgeMulti(), m_w_edgeMulti(), - m_weightedError_edgeMulti() + : m_mapOfCameraTransformationMatrix(), m_mapOfEdgeTrackers(), m_mapOfPyramidalImages(), + m_referenceCameraName("Camera"), m_L_edgeMulti(), m_error_edgeMulti(), m_w_edgeMulti(), m_weightedError_edgeMulti() { if (nbCameras == 0) { - throw vpException( - vpTrackingException::fatalError, - "Cannot construct a vpMbEdgeMultiTracker with no camera !"); + throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbEdgeMultiTracker with no camera !"); } else if (nbCameras == 1) { m_mapOfEdgeTrackers["Camera"] = new vpMbEdgeTracker(); @@ -117,22 +111,16 @@ vpMbEdgeMultiTracker::vpMbEdgeMultiTracker(const unsigned int nbCameras) \param cameraNames : List of camera names. */ -vpMbEdgeMultiTracker::vpMbEdgeMultiTracker( - const std::vector &cameraNames) - : m_mapOfCameraTransformationMatrix(), m_mapOfEdgeTrackers(), - m_mapOfPyramidalImages(), m_referenceCameraName("Camera"), - m_L_edgeMulti(), m_error_edgeMulti(), m_w_edgeMulti(), - m_weightedError_edgeMulti() +vpMbEdgeMultiTracker::vpMbEdgeMultiTracker(const std::vector &cameraNames) + : m_mapOfCameraTransformationMatrix(), m_mapOfEdgeTrackers(), m_mapOfPyramidalImages(), + m_referenceCameraName("Camera"), m_L_edgeMulti(), m_error_edgeMulti(), m_w_edgeMulti(), m_weightedError_edgeMulti() { if (cameraNames.empty()) { - throw vpException( - vpTrackingException::fatalError, - "Cannot construct a vpMbEdgeMultiTracker with no camera !"); + throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbEdgeMultiTracker with no camera !"); } - for (std::vector::const_iterator it = cameraNames.begin(); - it != cameraNames.end(); ++it) { + for (std::vector::const_iterator it = cameraNames.begin(); it != cameraNames.end(); ++it) { m_mapOfEdgeTrackers[*it] = new vpMbEdgeTracker(); } @@ -145,8 +133,7 @@ vpMbEdgeMultiTracker::vpMbEdgeMultiTracker( */ vpMbEdgeMultiTracker::~vpMbEdgeMultiTracker() { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { delete it->second; } @@ -156,13 +143,9 @@ vpMbEdgeMultiTracker::~vpMbEdgeMultiTracker() cleanPyramid(m_mapOfPyramidalImages); } -void vpMbEdgeMultiTracker::cleanPyramid( - std::map *> > - &pyramid) +void vpMbEdgeMultiTracker::cleanPyramid(std::map *> > &pyramid) { - for (std::map *> >::iterator it1 = - pyramid.begin(); + for (std::map *> >::iterator it1 = pyramid.begin(); it1 != pyramid.end(); ++it1) { if (it1->second.size() > 0) { it1->second[0] = NULL; @@ -182,24 +165,20 @@ void vpMbEdgeMultiTracker::computeProjectionError() if (computeProjError) { double rawTotalProjectionError = 0.0; unsigned int nbTotalFeaturesUsed = 0; - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { double curProjError = it->second->getProjectionError(); - unsigned int nbFeaturesUsed = - it->second->nbFeaturesForProjErrorComputation; + unsigned int nbFeaturesUsed = it->second->nbFeaturesForProjErrorComputation; if (nbFeaturesUsed > 0) { nbTotalFeaturesUsed += nbFeaturesUsed; - rawTotalProjectionError += - (vpMath::rad(curProjError) * nbFeaturesUsed); + rawTotalProjectionError += (vpMath::rad(curProjError) * nbFeaturesUsed); } } if (nbTotalFeaturesUsed > 0) { nbFeaturesForProjErrorComputation = nbTotalFeaturesUsed; - projectionError = - vpMath::deg(rawTotalProjectionError / (double)nbTotalFeaturesUsed); + projectionError = vpMath::deg(rawTotalProjectionError / (double)nbTotalFeaturesUsed); } else { nbFeaturesForProjErrorComputation = 0; projectionError = 90.0; @@ -207,9 +186,8 @@ void vpMbEdgeMultiTracker::computeProjectionError() } } -void vpMbEdgeMultiTracker::computeVVS( - std::map *> &mapOfImages, - const unsigned int lvl) +void vpMbEdgeMultiTracker::computeVVS(std::map *> &mapOfImages, + const unsigned int lvl) { computeVVSInit(); unsigned int nbrow = m_error_edgeMulti.getRows(); @@ -218,12 +196,10 @@ void vpMbEdgeMultiTracker::computeVVS( // Parametre pour la premiere phase d'asservissement bool reloop = true; - bool isoJoIdentity_ = - isoJoIdentity; // Backup since it can be modified if L is not full rank + bool isoJoIdentity_ = isoJoIdentity; // Backup since it can be modified if L is not full rank std::map mapOfVelocityTwist; - for (std::map::const_iterator it = - m_mapOfCameraTransformationMatrix.begin(); + for (std::map::const_iterator it = m_mapOfCameraTransformationMatrix.begin(); it != m_mapOfCameraTransformationMatrix.end(); ++it) { vpVelocityTwistMatrix cVo; cVo.buildFrom(it->second); @@ -237,8 +213,7 @@ void vpMbEdgeMultiTracker::computeVVS( vpMbEdgeTracker *edge; while (reloop == true && iter < 10) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo; } @@ -247,17 +222,14 @@ void vpMbEdgeMultiTracker::computeVVS( reloop = false; unsigned int start_idx = 0; - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { double count_tmp = 0.0; edge = it->second; - edge->computeVVSFirstPhase(*mapOfImages[it->first], iter, count_tmp, - lvl); + edge->computeVVSFirstPhase(*mapOfImages[it->first], iter, count_tmp, lvl); count += count_tmp; - m_L_edgeMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], - start_idx, 0); + m_L_edgeMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], start_idx, 0); m_factor.insert(start_idx, edge->m_factor); m_w_edgeMulti.insert(start_idx, edge->m_w_edge); m_error_edgeMulti.insert(start_idx, edge->m_error_edge); @@ -281,8 +253,7 @@ void vpMbEdgeMultiTracker::computeVVS( /*** Second phase ***/ - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { edge = it->second; edge->m_w_edge = 1; @@ -308,14 +279,11 @@ void vpMbEdgeMultiTracker::computeVVS( vpMatrix LTL; // while ( ((int)((residu_1 - r)*1e8) != 0 ) && (iter<30)) - while (std::fabs((residu_1 - r) * 1e8) > - std::numeric_limits::epsilon() && - (iter < m_maxIter)) { + while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits::epsilon() && (iter < m_maxIter)) { computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist); bool reStartFromLastIncrement = false; - computeVVSCheckLevenbergMarquardt(iter, m_error_edgeMulti, m_error_prev, - cMoPrev, mu, reStartFromLastIncrement, + computeVVSCheckLevenbergMarquardt(iter, m_error_edgeMulti, m_error_prev, cMoPrev, mu, reStartFromLastIncrement, &m_w_prev); if (!reStartFromLastIncrement) { @@ -363,10 +331,8 @@ void vpMbEdgeMultiTracker::computeVVS( residu_1 = r; r = sqrt(num / den); // Le critere d'arret prend en compte le poids - computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_edgeMulti, LTL, - m_weightedError_edgeMulti, m_error_edgeMulti, - m_error_prev, LTR, mu, v, &m_w_edgeMulti, - &m_w_prev); + computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_edgeMulti, LTL, m_weightedError_edgeMulti, m_error_edgeMulti, + m_error_prev, LTR, mu, v, &m_w_edgeMulti, &m_w_prev); cMoPrev = cMo; cMo = vpExponentialMap::direct(v).inverse() * cMo; @@ -377,19 +343,16 @@ void vpMbEdgeMultiTracker::computeVVS( // std::cout << "VVS estimate pose cMo:\n" << cMo << std::endl; - computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMoPrev, L_true, - LVJ_true, m_error_edgeMulti); + computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMoPrev, L_true, LVJ_true, m_error_edgeMulti); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { edge = it->second; edge->updateMovingEdgeWeights(); } } -void vpMbEdgeMultiTracker::computeVVSFirstPhasePoseEstimation( - const unsigned int iter, bool &isoJoIdentity_) +void vpMbEdgeMultiTracker::computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_) { unsigned int nerror = m_weightedError_edgeMulti.getRows(); @@ -426,8 +389,7 @@ void vpMbEdgeMultiTracker::computeVVSFirstPhasePoseEstimation( vpMatrix K; // kernel unsigned int rank = (m_L_edgeMulti * cVo).kernel(K); if (rank == 0) { - throw vpException(vpException::fatalError, - "Rank=0, cannot estimate the pose !"); + throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !"); } if (rank != 6) { vpMatrix I; // Identity @@ -445,20 +407,14 @@ void vpMbEdgeMultiTracker::computeVVSFirstPhasePoseEstimation( if (isoJoIdentity_) { LTL = m_L_edgeMulti.AtA(); computeJTR(m_L_edgeMulti, m_weightedError_edgeMulti, LTR); - v = -0.7 * - LTL.pseudoInverse(LTL.getRows() * - std::numeric_limits::epsilon()) * - LTR; + v = -0.7 * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits::epsilon()) * LTR; } else { cVo.buildFrom(cMo); vpMatrix LVJ = (m_L_edgeMulti * cVo * oJo); vpMatrix LVJTLVJ = (LVJ).AtA(); vpColVector LVJTR; computeJTR(LVJ, m_weightedError_edgeMulti, LVJTR); - v = -0.7 * - LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * - std::numeric_limits::epsilon()) * - LVJTR; + v = -0.7 * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits::epsilon()) * LVJTR; v = cVo * v; } @@ -469,8 +425,7 @@ void vpMbEdgeMultiTracker::computeVVSInit() { unsigned int nbrow = 0; - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { vpMbEdgeTracker *edge = it->second; @@ -482,9 +437,8 @@ void vpMbEdgeMultiTracker::computeVVSInit() } if (nbrow < 4) { - throw vpTrackingException( - vpTrackingException::notEnoughPointError, - "No data found to compute the interaction matrix..."); + throw vpTrackingException(vpTrackingException::notEnoughPointError, + "No data found to compute the interaction matrix..."); } // Initialize with correct size @@ -508,16 +462,14 @@ void vpMbEdgeMultiTracker::computeVVSInteractionMatrixAndResidu( { unsigned int start_idx = 0; - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { vpMbEdgeTracker *edge = it->second; edge->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo; edge->computeVVSInteractionMatrixAndResidu(*mapOfImages[it->first]); - m_L_edgeMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], - start_idx, 0); + m_L_edgeMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], start_idx, 0); m_error_edgeMulti.insert(start_idx, edge->m_error_edge); start_idx += edge->m_error_edge.getRows(); @@ -528,8 +480,7 @@ void vpMbEdgeMultiTracker::computeVVSWeights() { unsigned int start_idx = 0; - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { vpMbEdgeTracker *edge = it->second; @@ -552,21 +503,16 @@ void vpMbEdgeMultiTracker::computeVVSWeights() \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeMultiTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, - const vpColor &col, - const unsigned int thickness, +void vpMbEdgeMultiTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->display(I, cMo_, cam_, col, thickness, displayFullModel); } else { - std::cerr << "Cannot find reference camera: " << m_referenceCameraName - << " !" << std::endl; + std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl; } } @@ -581,21 +527,16 @@ void vpMbEdgeMultiTracker::display(const vpImage &I, \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeMultiTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, - const vpColor &col, - const unsigned int thickness, +void vpMbEdgeMultiTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->display(I, cMo_, cam_, col, thickness, displayFullModel); } else { - std::cerr << "Cannot find reference camera: " << m_referenceCameraName - << " !" << std::endl; + std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl; } } @@ -614,24 +555,21 @@ void vpMbEdgeMultiTracker::display(const vpImage &I, \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeMultiTracker::display( - const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, const vpCameraParameters &cam2, - const vpColor &col, const unsigned int thickness, - const bool displayFullModel) +void vpMbEdgeMultiTracker::display(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, + const vpCameraParameters &cam1, const vpCameraParameters &cam2, const vpColor &col, + const unsigned int thickness, const bool displayFullModel) { if (m_mapOfEdgeTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it->second->display(I1, c1Mo, cam1, col, thickness, displayFullModel); ++it; it->second->display(I2, c2Mo, cam2, col, thickness, displayFullModel); } else { - std::cerr << "This display is only for the stereo case ! There are " - << m_mapOfEdgeTrackers.size() << " camera !" << std::endl; + std::cerr << "This display is only for the stereo case ! There are " << m_mapOfEdgeTrackers.size() << " camera !" + << std::endl; } } @@ -650,24 +588,21 @@ void vpMbEdgeMultiTracker::display( \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeMultiTracker::display( - const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, const vpCameraParameters &cam2, - const vpColor &col, const unsigned int thickness, - const bool displayFullModel) +void vpMbEdgeMultiTracker::display(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, + const vpCameraParameters &cam1, const vpCameraParameters &cam2, const vpColor &col, + const unsigned int thickness, const bool displayFullModel) { if (m_mapOfEdgeTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it->second->display(I1, c1Mo, cam1, col, thickness, displayFullModel); ++it; it->second->display(I2, c2Mo, cam2, col, thickness, displayFullModel); } else { - std::cerr << "This display is only for the stereo case ! There are " - << m_mapOfEdgeTrackers.size() << " cameras !" << std::endl; + std::cerr << "This display is only for the stereo case ! There are " << m_mapOfEdgeTrackers.size() << " cameras !" + << std::endl; } } @@ -683,31 +618,22 @@ void vpMbEdgeMultiTracker::display( \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeMultiTracker::display( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness, - const bool displayFullModel) +void vpMbEdgeMultiTracker::display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, + const vpColor &col, const unsigned int thickness, const bool displayFullModel) { // Display only for the given images - for (std::map *>::const_iterator - it_img = mapOfImages.begin(); + for (std::map *>::const_iterator it_img = mapOfImages.begin(); it_img != mapOfImages.end(); ++it_img) { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(it_img->first); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(it_img->first); - std::map::const_iterator it_cam = - mapOfCameraParameters.find(it_img->first); - - if (it_edge != m_mapOfEdgeTrackers.end() && - it_camPose != mapOfCameraPoses.end() && + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(it_img->first); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first); + std::map::const_iterator it_cam = mapOfCameraParameters.find(it_img->first); + + if (it_edge != m_mapOfEdgeTrackers.end() && it_camPose != mapOfCameraPoses.end() && it_cam != mapOfCameraParameters.end()) { - it_edge->second->display(*it_img->second, it_camPose->second, - it_cam->second, col, thickness, - displayFullModel); + it_edge->second->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel); } else { std::cerr << "Missing elements !" << std::endl; } @@ -726,31 +652,22 @@ void vpMbEdgeMultiTracker::display( \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeMultiTracker::display( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness, - const bool displayFullModel) +void vpMbEdgeMultiTracker::display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, + const vpColor &col, const unsigned int thickness, const bool displayFullModel) { // Display only for the given images - for (std::map *>::const_iterator it_img = - mapOfImages.begin(); + for (std::map *>::const_iterator it_img = mapOfImages.begin(); it_img != mapOfImages.end(); ++it_img) { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(it_img->first); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(it_img->first); - std::map::const_iterator it_cam = - mapOfCameraParameters.find(it_img->first); - - if (it_edge != m_mapOfEdgeTrackers.end() && - it_camPose != mapOfCameraPoses.end() && + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(it_img->first); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first); + std::map::const_iterator it_cam = mapOfCameraParameters.find(it_img->first); + + if (it_edge != m_mapOfEdgeTrackers.end() && it_camPose != mapOfCameraPoses.end() && it_cam != mapOfCameraParameters.end()) { - it_edge->second->display(*it_img->second, it_camPose->second, - it_cam->second, col, thickness, - displayFullModel); + it_edge->second->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel); } else { std::cerr << "Missing elements !" << std::endl; } @@ -766,8 +683,7 @@ std::vector vpMbEdgeMultiTracker::getCameraNames() const { std::vector cameraNames; - for (std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { cameraNames.push_back(it_edge->first); } @@ -780,17 +696,14 @@ std::vector vpMbEdgeMultiTracker::getCameraNames() const \param camera : Copy of the camera parameters used by the tracker. */ -void vpMbEdgeMultiTracker::getCameraParameters( - vpCameraParameters &camera) const +void vpMbEdgeMultiTracker::getCameraParameters(vpCameraParameters &camera) const { // Get the reference camera parameters - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->getCameraParameters(camera); } else { - std::cerr << "The reference camera name: " << m_referenceCameraName - << " does not exist !" << std::endl; + std::cerr << "The reference camera name: " << m_referenceCameraName << " does not exist !" << std::endl; } } @@ -800,19 +713,17 @@ void vpMbEdgeMultiTracker::getCameraParameters( \param cam1 : Copy of the camera parameters for the first camera. \param cam2 : Copy of the camera parameters for the second camera. */ -void vpMbEdgeMultiTracker::getCameraParameters(vpCameraParameters &cam1, - vpCameraParameters &cam2) const +void vpMbEdgeMultiTracker::getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const { if (m_mapOfEdgeTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it->second->getCameraParameters(cam1); ++it; it->second->getCameraParameters(cam2); } else { - std::cerr << "Problem with the number of cameras ! There are " - << m_mapOfEdgeTrackers.size() << " cameras !" << std::endl; + std::cerr << "Problem with the number of cameras ! There are " << m_mapOfEdgeTrackers.size() << " cameras !" + << std::endl; } } @@ -822,16 +733,13 @@ void vpMbEdgeMultiTracker::getCameraParameters(vpCameraParameters &cam1, \param cameraName : Name of the camera. \param camera : Copy of the camera parameters. */ -void vpMbEdgeMultiTracker::getCameraParameters( - const std::string &cameraName, vpCameraParameters &camera) const +void vpMbEdgeMultiTracker::getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->getCameraParameters(camera); } else { - std::cerr << "The camera: " << cameraName << " does not exist !" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl; } } @@ -840,14 +748,12 @@ void vpMbEdgeMultiTracker::getCameraParameters( \param mapOfCameraParameters : Map of camera parameters. */ -void vpMbEdgeMultiTracker::getCameraParameters( - std::map &mapOfCameraParameters) const +void vpMbEdgeMultiTracker::getCameraParameters(std::map &mapOfCameraParameters) const { // Clear the input map mapOfCameraParameters.clear(); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { vpCameraParameters cam_; it->second->getCameraParameters(cam_); @@ -862,11 +768,9 @@ void vpMbEdgeMultiTracker::getCameraParameters( \param cameraName : Name of the desired camera. \return Clipping flags. */ -unsigned int -vpMbEdgeMultiTracker::getClipping(const std::string &cameraName) const +unsigned int vpMbEdgeMultiTracker::getClipping(const std::string &cameraName) const { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { return it->second->getClipping(); } else { @@ -883,14 +787,12 @@ vpMbEdgeMultiTracker::getClipping(const std::string &cameraName) const */ vpMbHiddenFaces &vpMbEdgeMultiTracker::getFaces() { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { return it->second->getFaces(); } - std::cerr << "The reference camera: " << m_referenceCameraName - << " cannot be found !" << std::endl; + std::cerr << "The reference camera: " << m_referenceCameraName << " cannot be found !" << std::endl; return faces; } @@ -899,17 +801,14 @@ vpMbHiddenFaces &vpMbEdgeMultiTracker::getFaces() \return Reference to the face structure. */ -vpMbHiddenFaces & -vpMbEdgeMultiTracker::getFaces(const std::string &cameraName) +vpMbHiddenFaces &vpMbEdgeMultiTracker::getFaces(const std::string &cameraName) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { return it->second->getFaces(); } - std::cerr << "The camera: " << cameraName << " cannot be found !" - << std::endl; + std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl; return faces; } @@ -918,12 +817,10 @@ vpMbEdgeMultiTracker::getFaces(const std::string &cameraName) \return Reference a map of the face structure for each camera. */ -std::map > -vpMbEdgeMultiTracker::getFaces() const +std::map > vpMbEdgeMultiTracker::getFaces() const { std::map > mapOfFaces; - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { mapOfFaces[it->first] = it->second->faces; } @@ -941,18 +838,14 @@ vpMbEdgeMultiTracker::getFaces() const \param level : Level corresponding to the list to return. \param circlesList : The list of the circles of the model. */ -void vpMbEdgeMultiTracker::getLcircle( - std::list &circlesList, - const unsigned int level) const +void vpMbEdgeMultiTracker::getLcircle(std::list &circlesList, const unsigned int level) const { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it_edge != m_mapOfEdgeTrackers.end()) { it_edge->second->getLcircle(circlesList, level); } else { - std::cerr << "Cannot find reference camera: " << m_referenceCameraName - << " !" << std::endl; + std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl; } } @@ -967,18 +860,14 @@ void vpMbEdgeMultiTracker::getLcircle( vpMbtDistanceCircle. \param level : Level corresponding to the list to return. \param circlesList : The list of the circles of the model. */ -void vpMbEdgeMultiTracker::getLcircle( - const std::string &cameraName, - std::list &circlesList, - const unsigned int level) const +void vpMbEdgeMultiTracker::getLcircle(const std::string &cameraName, std::list &circlesList, + const unsigned int level) const { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->getLcircle(circlesList, level); } else { - std::cerr << "The camera: " << cameraName << " does not exist !" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl; } } @@ -992,18 +881,15 @@ void vpMbEdgeMultiTracker::getLcircle( \param level : Level corresponding to the list to return. \param cylindersList : The list of the cylinders of the model. */ -void vpMbEdgeMultiTracker::getLcylinder( - std::list &cylindersList, - const unsigned int level) const +void vpMbEdgeMultiTracker::getLcylinder(std::list &cylindersList, + const unsigned int level) const { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it_edge != m_mapOfEdgeTrackers.end()) { it_edge->second->getLcylinder(cylindersList, level); } else { - std::cerr << "Cannot find reference camera: " << m_referenceCameraName - << " !" << std::endl; + std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl; } } @@ -1018,18 +904,15 @@ void vpMbEdgeMultiTracker::getLcylinder( vpMbtDistanceCylinder. \param level : Level corresponding to the list to return. \param cylindersList : The list of the cylinders of the model. */ -void vpMbEdgeMultiTracker::getLcylinder( - const std::string &cameraName, - std::list &cylindersList, - const unsigned int level) const +void vpMbEdgeMultiTracker::getLcylinder(const std::string &cameraName, + std::list &cylindersList, + const unsigned int level) const { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->getLcylinder(cylindersList, level); } else { - std::cerr << "The camera: " << cameraName << " does not exist !" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl; } } @@ -1043,17 +926,14 @@ void vpMbEdgeMultiTracker::getLcylinder( \param level : Level corresponding to the list to return. \param linesList : The list of the lines of the model. */ -void vpMbEdgeMultiTracker::getLline(std::list &linesList, - const unsigned int level) const +void vpMbEdgeMultiTracker::getLline(std::list &linesList, const unsigned int level) const { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it_edge != m_mapOfEdgeTrackers.end()) { it_edge->second->getLline(linesList, level); } else { - std::cerr << "Cannot find reference camera: " << m_referenceCameraName - << " !" << std::endl; + std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl; } } @@ -1068,17 +948,14 @@ void vpMbEdgeMultiTracker::getLline(std::list &linesList, vpMbtDistanceLine. \param level : Level corresponding to the list to return. \param linesList : The list of the lines of the model. */ -void vpMbEdgeMultiTracker::getLline(const std::string &cameraName, - std::list &linesList, +void vpMbEdgeMultiTracker::getLline(const std::string &cameraName, std::list &linesList, const unsigned int level) const { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->getLline(linesList, level); } else { - std::cerr << "The camera: " << cameraName << " does not exist !" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl; } } @@ -1090,13 +967,11 @@ void vpMbEdgeMultiTracker::getLline(const std::string &cameraName, */ void vpMbEdgeMultiTracker::getMovingEdge(vpMe &p_me) const { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it_edge != m_mapOfEdgeTrackers.end()) { it_edge->second->getMovingEdge(p_me); } else { - std::cerr << "The reference camera: " << m_referenceCameraName - << " does not exist !" << std::endl; + std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist !" << std::endl; } } @@ -1107,14 +982,12 @@ void vpMbEdgeMultiTracker::getMovingEdge(vpMe &p_me) const */ vpMe vpMbEdgeMultiTracker::getMovingEdge() const { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); vpMe me_tmp; if (it_edge != m_mapOfEdgeTrackers.end()) { it_edge->second->getMovingEdge(me_tmp); } else { - std::cerr << "The reference camera: " << m_referenceCameraName - << " does not exist !" << std::endl; + std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist !" << std::endl; } return me_tmp; @@ -1127,16 +1000,13 @@ vpMe vpMbEdgeMultiTracker::getMovingEdge() const \param p_me : Moving edge parameters for the specified camera name. */ -void vpMbEdgeMultiTracker::getMovingEdge(const std::string &cameraName, - vpMe &p_me) const +void vpMbEdgeMultiTracker::getMovingEdge(const std::string &cameraName, vpMe &p_me) const { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->getMovingEdge(p_me); } else { - std::cerr << "The camera: " << cameraName << " does not exist !" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl; } } @@ -1168,16 +1038,14 @@ vpMe vpMbEdgeMultiTracker::getMovingEdge(const std::string &cameraName) const */ unsigned int vpMbEdgeMultiTracker::getNbPoints(const unsigned int level) const { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); unsigned int nbGoodPoints = 0; if (it_edge != m_mapOfEdgeTrackers.end()) { nbGoodPoints += it_edge->second->getNbPoints(level); } else { - std::cerr << "The reference camera: " << m_referenceCameraName - << " does not exist !" << std::endl; + std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist !" << std::endl; } return nbGoodPoints; @@ -1196,16 +1064,13 @@ unsigned int vpMbEdgeMultiTracker::getNbPoints(const unsigned int level) const \return the number of good points for the specified camera name. */ -unsigned int vpMbEdgeMultiTracker::getNbPoints(const std::string &cameraName, - const unsigned int level) const +unsigned int vpMbEdgeMultiTracker::getNbPoints(const std::string &cameraName, const unsigned int level) const { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { return it->second->getNbPoints(level); } else { - std::cerr << "The camera: " << cameraName << " does not exist !" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl; } return 0; @@ -1218,14 +1083,12 @@ unsigned int vpMbEdgeMultiTracker::getNbPoints(const std::string &cameraName, */ unsigned int vpMbEdgeMultiTracker::getNbPolygon() const { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { return it->second->getNbPolygon(); } - std::cerr << "The reference camera: " << m_referenceCameraName - << " cannot be found !" << std::endl; + std::cerr << "The reference camera: " << m_referenceCameraName << " cannot be found !" << std::endl; return 0; } @@ -1235,17 +1098,14 @@ unsigned int vpMbEdgeMultiTracker::getNbPolygon() const \return Number of polygons for the specified camera. */ -unsigned int -vpMbEdgeMultiTracker::getNbPolygon(const std::string &cameraName) const +unsigned int vpMbEdgeMultiTracker::getNbPolygon(const std::string &cameraName) const { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { return it->second->getNbPolygon(); } - std::cerr << "The camera: " << cameraName << " cannot be found !" - << std::endl; + std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl; return 0; } @@ -1255,12 +1115,10 @@ vpMbEdgeMultiTracker::getNbPolygon(const std::string &cameraName) const \return Number of polygons for the specified camera. */ -std::map -vpMbEdgeMultiTracker::getMultiNbPolygon() const +std::map vpMbEdgeMultiTracker::getMultiNbPolygon() const { std::map mapOfNbPolygons; - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { mapOfNbPolygons[it->first] = it->second->getNbPolygon(); } @@ -1274,19 +1132,16 @@ vpMbEdgeMultiTracker::getMultiNbPolygon() const \param c1Mo : The camera pose for the first camera. \param c2Mo : The camera pose for the second camera. */ -void vpMbEdgeMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, - vpHomogeneousMatrix &c2Mo) const +void vpMbEdgeMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const { if (m_mapOfEdgeTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it->second->getPose(c1Mo); ++it; it->second->getPose(c2Mo); } else { - std::cerr << "Require two cameras ! There are " - << m_mapOfEdgeTrackers.size() << " cameras !" << std::endl; + std::cerr << "Require two cameras ! There are " << m_mapOfEdgeTrackers.size() << " cameras !" << std::endl; } } @@ -1298,16 +1153,13 @@ void vpMbEdgeMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, \param cameraName : The name of the camera. \param cMo_ : The camera pose for the specified camera. */ -void vpMbEdgeMultiTracker::getPose(const std::string &cameraName, - vpHomogeneousMatrix &cMo_) const +void vpMbEdgeMultiTracker::getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo_) const { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->getPose(cMo_); } else { - std::cerr << "The camera: " << cameraName << " does not exist !" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl; } } @@ -1316,14 +1168,12 @@ void vpMbEdgeMultiTracker::getPose(const std::string &cameraName, \param mapOfCameraPoses : The map of camera poses for all the cameras. */ -void vpMbEdgeMultiTracker::getPose( - std::map &mapOfCameraPoses) const +void vpMbEdgeMultiTracker::getPose(std::map &mapOfCameraPoses) const { // Clear the map mapOfCameraPoses.clear(); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { vpHomogeneousMatrix cMo_; it->second->getPose(cMo_); @@ -1344,27 +1194,22 @@ void vpMbEdgeMultiTracker::init(const vpImage & /*I*/) {} image used to display the help. This image may be used to show where to click. This functionality is only available if visp_io module is used. */ -void vpMbEdgeMultiTracker::initClick( - const vpImage &I, - const std::vector &points3D_list, const std::string &displayFile) +void vpMbEdgeMultiTracker::initClick(const vpImage &I, const std::vector &points3D_list, + const std::string &displayFile) { if (m_mapOfEdgeTrackers.empty()) { - throw vpException(vpTrackingException::initializationError, - "There is no camera !"); + throw vpException(vpTrackingException::initializationError, "There is no camera !"); } else if (m_mapOfEdgeTrackers.size() > 1) { - throw vpException(vpTrackingException::initializationError, - "There is more than one camera !"); + throw vpException(vpTrackingException::initializationError, "There is more than one camera !"); } else { // Get the vpMbEdgeTracker object for the reference camera name - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->initClick(I, points3D_list, displayFile); it->second->getPose(cMo); } else { std::stringstream ss; - ss << "Cannot initClick as the reference camera: " - << m_referenceCameraName << " does not exist !"; + ss << "Cannot initClick as the reference camera: " << m_referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } @@ -1390,7 +1235,8 @@ void vpMbEdgeMultiTracker::initClick( \param I : Input image where the user has to click. \param initFile : File containing the coordinates of at least 4 3D points the user has to click in the image. This file should have .init extension - (ie teabox.init). \param displayHelp : Optionnal display of an image that + (ie teabox.init). + \param displayHelp : Optionnal display of an image that should have the same generic name as the init file (ie teabox.ppm). This image may be used to show where to click. This functionality is only available if visp_io module is used. @@ -1398,27 +1244,22 @@ void vpMbEdgeMultiTracker::initClick( \exception vpException::ioError : The file specified in \e initFile doesn't exist. */ -void vpMbEdgeMultiTracker::initClick(const vpImage &I, - const std::string &initFile, +void vpMbEdgeMultiTracker::initClick(const vpImage &I, const std::string &initFile, const bool displayHelp) { if (m_mapOfEdgeTrackers.empty()) { - throw vpException(vpTrackingException::initializationError, - "There is no camera !"); + throw vpException(vpTrackingException::initializationError, "There is no camera !"); } else if (m_mapOfEdgeTrackers.size() > 1) { - throw vpException(vpTrackingException::initializationError, - "There is more than one camera !"); + throw vpException(vpTrackingException::initializationError, "There is more than one camera !"); } else { // Get the vpMbEdgeTracker object for the reference camera name - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->initClick(I, initFile, displayHelp); it->second->getPose(cMo); } else { std::stringstream ss; - ss << "Cannot initClick as the reference camera: " - << m_referenceCameraName << " does not exist !"; + ss << "Cannot initClick as the reference camera: " << m_referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } @@ -1459,16 +1300,12 @@ void vpMbEdgeMultiTracker::initClick(const vpImage &I, \exception vpException::ioError : The file specified in \e initFile doesn't exist. */ -void vpMbEdgeMultiTracker::initClick(const vpImage &I1, - const vpImage &I2, - const std::string &initFile1, - const std::string &initFile2, - const bool displayHelp, +void vpMbEdgeMultiTracker::initClick(const vpImage &I1, const vpImage &I2, + const std::string &initFile1, const std::string &initFile2, const bool displayHelp, const bool firstCameraIsReference) { if (m_mapOfEdgeTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it->second->initClick(I1, initFile1, displayHelp); if (firstCameraIsReference) { @@ -1492,8 +1329,7 @@ void vpMbEdgeMultiTracker::initClick(const vpImage &I1, } } else { std::stringstream ss; - ss << "Cannot init click ! Require two cameras but there are " - << m_mapOfEdgeTrackers.size() << " cameras !"; + ss << "Cannot init click ! Require two cameras but there are " << m_mapOfEdgeTrackers.size() << " cameras !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } @@ -1517,7 +1353,8 @@ void vpMbEdgeMultiTracker::initClick(const vpImage &I1, \param mapOfImages : Map of images. \param initFile : File containing the points where to click for the - reference camera. \param displayHelp : Optionnal display of an image that + reference camera. + \param displayHelp : Optionnal display of an image that should have the same generic name as the init file (ie teabox.ppm). This image may be used to show where to click. This functionality is only available if visp_io module is used. @@ -1526,15 +1363,13 @@ void vpMbEdgeMultiTracker::initClick(const vpImage &I1, exist. */ -void vpMbEdgeMultiTracker::initClick( - const std::map *> &mapOfImages, - const std::string &initFile, const bool displayHelp) +void vpMbEdgeMultiTracker::initClick(const std::map *> &mapOfImages, + const std::string &initFile, const bool displayHelp) { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it_edge != m_mapOfEdgeTrackers.end()) { - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); if (it_img != mapOfImages.end()) { // Init click on reference camera @@ -1544,37 +1379,30 @@ void vpMbEdgeMultiTracker::initClick( it_edge->second->getPose(cMo); // Set the pose for the others cameras - for (it_edge = m_mapOfEdgeTrackers.begin(); - it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { + for (it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { if (it_edge->first != m_referenceCameraName) { it_img = mapOfImages.find(it_edge->first); - std::map::const_iterator - it_camTrans = - m_mapOfCameraTransformationMatrix.find(it_edge->first); + std::map::const_iterator it_camTrans = + m_mapOfCameraTransformationMatrix.find(it_edge->first); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; it_edge->second->setPose(*it_img->second, cCurrentMo); } else { std::stringstream ss; - ss << "Cannot init click ! Missing image for camera: " - << m_referenceCameraName << " !"; - throw vpException(vpTrackingException::initializationError, - ss.str()); + ss << "Cannot init click ! Missing image for camera: " << m_referenceCameraName << " !"; + throw vpException(vpTrackingException::initializationError, ss.str()); } } } } else { std::stringstream ss; - ss << "Cannot init click ! Missing image for camera: " - << m_referenceCameraName << " !"; + ss << "Cannot init click ! Missing image for camera: " << m_referenceCameraName << " !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } else { std::stringstream ss; - ss << "Cannot init click ! The reference camera: " - << m_referenceCameraName << " does not exist !"; + ss << "Cannot init click ! The reference camera: " << m_referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } @@ -1601,51 +1429,42 @@ void vpMbEdgeMultiTracker::initClick( \param mapOfImages : Map of images. \param mapOfInitFiles : map of files containing the points where to click - for each camera. \param displayHelp : Optional display of an image (ie + for each camera. + \param displayHelp : Optional display of an image (ie teabox.ppm). This image may be used to show where to click. \exception vpException::ioError : The file specified in \e initFile doesn't exist. */ -void vpMbEdgeMultiTracker::initClick( - const std::map *> &mapOfImages, - const std::map &mapOfInitFiles, - const bool displayHelp) +void vpMbEdgeMultiTracker::initClick(const std::map *> &mapOfImages, + const std::map &mapOfInitFiles, const bool displayHelp) { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_initFile = - mapOfInitFiles.find(m_referenceCameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_initFile = mapOfInitFiles.find(m_referenceCameraName); - if (it_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && - it_initFile != mapOfInitFiles.end()) { + if (it_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) { // InitClick for the reference camera - it_edge->second->initClick(*it_img->second, it_initFile->second, - displayHelp); + it_edge->second->initClick(*it_img->second, it_initFile->second, displayHelp); // Get reference camera pose it_edge->second->getPose(cMo); } else { - throw vpException(vpTrackingException::initializationError, - "Cannot initClick for the reference camera !"); + throw vpException(vpTrackingException::initializationError, "Cannot initClick for the reference camera !"); } // Vector of missing pose matrices for cameras std::vector vectorOfMissingCameraPoses; // InitClick for all the cameras that have an initFile - for (it_edge = m_mapOfEdgeTrackers.begin(); - it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { + for (it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { if (it_edge->first != m_referenceCameraName) { it_img = mapOfImages.find(it_edge->first); it_initFile = mapOfInitFiles.find(it_edge->first); - if (it_img != mapOfImages.end() && - it_initFile != mapOfInitFiles.end()) { - it_edge->second->initClick(*it_img->second, it_initFile->second, - displayHelp); + if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) { + it_edge->second->initClick(*it_img->second, it_initFile->second, displayHelp); } else { vectorOfMissingCameraPoses.push_back(it_edge->first); } @@ -1653,15 +1472,13 @@ void vpMbEdgeMultiTracker::initClick( } // SetPose for cameras that do not have an initFile - for (std::vector::const_iterator it1 = - vectorOfMissingCameraPoses.begin(); + for (std::vector::const_iterator it1 = vectorOfMissingCameraPoses.begin(); it1 != vectorOfMissingCameraPoses.end(); ++it1) { it_img = mapOfImages.find(*it1); std::map::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it1); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; m_mapOfEdgeTrackers[*it1]->setPose(*it_img->second, cCurrentMo); } else { @@ -1689,17 +1506,17 @@ void vpMbEdgeMultiTracker::initClick( \endcode Where the three firsts lines refer to the translation and the three last to - the rotation in thetaU parameterization (see vpThetaUVector). \param I : - Input image \param initFile : Path to the file containing the pose. + the rotation in thetaU parameterization (see vpThetaUVector). + + \param I : Input image + \param initFile : Path to the file containing the pose. */ -void vpMbEdgeMultiTracker::initFromPose(const vpImage &I, - const std::string &initFile) +void vpMbEdgeMultiTracker::initFromPose(const vpImage &I, const std::string &initFile) { // Monocular case only ! if (m_mapOfEdgeTrackers.size() > 1) { - throw vpException( - vpTrackingException::initializationError, - "This method can only be used for the monocular case !"); + throw vpException(vpTrackingException::initializationError, + "This method can only be used for the monocular case !"); } char s[FILENAME_MAX]; @@ -1728,11 +1545,9 @@ void vpMbEdgeMultiTracker::initFromPose(const vpImage &I, cMo.buildFrom(init_pos); // Init for the reference camera - std::map::iterator it_ref = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::iterator it_ref = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it_ref == m_mapOfEdgeTrackers.end()) { - throw vpException(vpTrackingException::initializationError, - "Cannot find the reference camera !"); + throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !"); } it_ref->second->cMo = cMo; @@ -1745,24 +1560,20 @@ void vpMbEdgeMultiTracker::initFromPose(const vpImage &I, \param I : Input image \param cMo_ : Pose matrix. */ -void vpMbEdgeMultiTracker::initFromPose(const vpImage &I, - const vpHomogeneousMatrix &cMo_) +void vpMbEdgeMultiTracker::initFromPose(const vpImage &I, const vpHomogeneousMatrix &cMo_) { if (m_mapOfEdgeTrackers.size() != 1) { std::stringstream ss; - ss << "This method requires exactly one camera, there are " - << m_mapOfEdgeTrackers.size() << " cameras !"; + ss << "This method requires exactly one camera, there are " << m_mapOfEdgeTrackers.size() << " cameras !"; throw vpException(vpTrackingException::initializationError, ss.str()); } this->cMo = cMo_; // Init for the reference camera - std::map::iterator it_ref = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::iterator it_ref = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it_ref == m_mapOfEdgeTrackers.end()) { - throw vpException(vpTrackingException::initializationError, - "Cannot find the reference camera !"); + throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !"); } it_ref->second->cMo = cMo; @@ -1775,8 +1586,7 @@ void vpMbEdgeMultiTracker::initFromPose(const vpImage &I, \param I : Input image \param cPo : Pose vector. */ -void vpMbEdgeMultiTracker::initFromPose(const vpImage &I, - const vpPoseVector &cPo) +void vpMbEdgeMultiTracker::initFromPose(const vpImage &I, const vpPoseVector &cPo) { vpHomogeneousMatrix _cMo(cPo); vpMbEdgeMultiTracker::initFromPose(I, _cMo); @@ -1792,10 +1602,8 @@ void vpMbEdgeMultiTracker::initFromPose(const vpImage &I, \param firstCameraIsReference : If true, the first camera is the reference camera, otherwise it is the second one. */ -void vpMbEdgeMultiTracker::initFromPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, +void vpMbEdgeMultiTracker::initFromPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference) { // For Edge, initFromPose has the same behavior than setPose @@ -1810,9 +1618,8 @@ void vpMbEdgeMultiTracker::initFromPose(const vpImage &I1, \param mapOfImages : Map of images. \param cMo_ : Pose matrix for the reference camera. */ -void vpMbEdgeMultiTracker::initFromPose( - const std::map *> &mapOfImages, - const vpHomogeneousMatrix &cMo_) +void vpMbEdgeMultiTracker::initFromPose(const std::map *> &mapOfImages, + const vpHomogeneousMatrix &cMo_) { // For Edge, initFromPose has the same behavior than setPose // So, for convenience we call setPose @@ -1825,22 +1632,18 @@ void vpMbEdgeMultiTracker::initFromPose( \param mapOfImages : Map of images. \param mapOfCameraPoses : Map of pose matrix. */ -void vpMbEdgeMultiTracker::initFromPose( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses) +void vpMbEdgeMultiTracker::initFromPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses) { // For Edge, initFromPose has the same behavior than setPose // So, for convenience we call setPose vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses); } -void vpMbEdgeMultiTracker::initPyramid( - const std::map *> &mapOfImages, - std::map *> > - &pyramid) +void vpMbEdgeMultiTracker::initPyramid(const std::map *> &mapOfImages, + std::map *> > &pyramid) { - for (std::map *>::const_iterator - it = mapOfImages.begin(); + for (std::map *>::const_iterator it = mapOfImages.begin(); it != mapOfImages.end(); ++it) { pyramid[it->first].resize(scales.size()); @@ -1900,8 +1703,7 @@ not found or wrong format for the data). */ void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { // Load ConfigFile for reference camera it->second->loadConfigFile(configFile); @@ -1916,8 +1718,7 @@ void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile) this->angleDisappears = it->second->getAngleDisappear(); } else { std::stringstream ss; - ss << "The reference camera: " << m_referenceCameraName - << " does not exist !"; + ss << "The reference camera: " << m_referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } @@ -1938,13 +1739,11 @@ void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile) \sa loadConfigFile(const std::string &), vpXmlParser::cleanup() */ -void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile1, - const std::string &configFile2, +void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2, const bool firstCameraIsReference) { if (m_mapOfEdgeTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it->second->loadConfigFile(configFile1); if (firstCameraIsReference) { @@ -1975,8 +1774,7 @@ void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile1, } } else { std::stringstream ss; - ss << "Cannot loadConfigFile. Require two cameras ! There are " - << m_mapOfEdgeTrackers.size() << " cameras !"; + ss << "Cannot loadConfigFile. Require two cameras ! There are " << m_mapOfEdgeTrackers.size() << " cameras !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } @@ -1994,27 +1792,22 @@ void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile1, \sa loadConfigFile(const std::string &), vpXmlParser::cleanup() */ -void vpMbEdgeMultiTracker::loadConfigFile( - const std::map &mapOfConfigFiles) +void vpMbEdgeMultiTracker::loadConfigFile(const std::map &mapOfConfigFiles) { - for (std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { - std::map::const_iterator it_config = - mapOfConfigFiles.find(it_edge->first); + std::map::const_iterator it_config = mapOfConfigFiles.find(it_edge->first); if (it_config != mapOfConfigFiles.end()) { it_edge->second->loadConfigFile(it_config->second); } else { std::stringstream ss; - ss << "Missing configuration file for camera: " << it_edge->first - << " !"; + ss << "Missing configuration file for camera: " << it_edge->first << " !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } // Set the reference camera parameters - std::map::iterator it = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->getCameraParameters(cam); @@ -2027,8 +1820,7 @@ void vpMbEdgeMultiTracker::loadConfigFile( this->angleDisappears = it->second->getAngleDisappear(); } else { std::stringstream ss; - ss << "The reference camera: " << m_referenceCameraName - << " does not exist !"; + ss << "The reference camera: " << m_referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } @@ -2058,11 +1850,9 @@ is not wrl or cao. \param verbose : verbose option to print additional information when loading CAO model files which include other CAO model files. */ -void vpMbEdgeMultiTracker::loadModel(const std::string &modelFile, - const bool verbose) +void vpMbEdgeMultiTracker::loadModel(const std::string &modelFile, const bool verbose) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->loadModel(modelFile, verbose); } @@ -2079,20 +1869,16 @@ void vpMbEdgeMultiTracker::loadModel(const std::string &modelFile, model \param verbose : verbose option to print additional information when loading CAO model files which include other CAO model files. */ -void vpMbEdgeMultiTracker::reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbEdgeMultiTracker::reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose) { if (m_mapOfEdgeTrackers.size() != 1) { std::stringstream ss; - ss << "This method requires exactly one camera, there are " - << m_mapOfEdgeTrackers.size() << " cameras !"; + ss << "This method requires exactly one camera, there are " << m_mapOfEdgeTrackers.size() << " cameras !"; throw vpException(vpTrackingException::fatalError, ss.str()); } - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it_edge != m_mapOfEdgeTrackers.end()) { it_edge->second->reInitModel(I, cad_name, cMo_, verbose); @@ -2116,17 +1902,13 @@ void vpMbEdgeMultiTracker::reInitModel(const vpImage &I, other CAO model files. \param firstCameraIsReference : If true, the first camera is the reference camera, otherwise it is the second one. */ -void vpMbEdgeMultiTracker::reInitModel(const vpImage &I1, - const vpImage &I2, - const std::string &cad_name, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, - const bool verbose, +void vpMbEdgeMultiTracker::reInitModel(const vpImage &I1, const vpImage &I2, + const std::string &cad_name, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const bool verbose, const bool firstCameraIsReference) { if (m_mapOfEdgeTrackers.size() == 2) { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.begin(); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.begin(); it_edge->second->reInitModel(I1, cad_name, c1Mo, verbose); @@ -2144,8 +1926,7 @@ void vpMbEdgeMultiTracker::reInitModel(const vpImage &I1, it_edge->second->getPose(cMo); } } else { - throw vpException(vpTrackingException::fatalError, - "This method requires exactly two cameras !"); + throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras !"); } } @@ -2159,61 +1940,49 @@ void vpMbEdgeMultiTracker::reInitModel(const vpImage &I1, additional information when loading CAO model files which include other CAO model files. */ -void vpMbEdgeMultiTracker::reInitModel( - const std::map *> &mapOfImages, - const std::string &cad_name, - const std::map &mapOfCameraPoses, - const bool verbose) -{ - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(m_referenceCameraName); - - if (it_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && - it_camPose != mapOfCameraPoses.end()) { - it_edge->second->reInitModel(*it_img->second, cad_name, - it_camPose->second, verbose); +void vpMbEdgeMultiTracker::reInitModel(const std::map *> &mapOfImages, + const std::string &cad_name, + const std::map &mapOfCameraPoses, + const bool verbose) +{ + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName); + + if (it_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) { + it_edge->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose); modelInitialised = true; // Set reference pose it_edge->second->getPose(cMo); } else { - throw vpException(vpTrackingException::fatalError, - "Cannot reInitModel for reference camera !"); + throw vpException(vpTrackingException::fatalError, "Cannot reInitModel for reference camera !"); } std::vector vectorOfMissingCameras; - for (it_edge = m_mapOfEdgeTrackers.begin(); - it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { + for (it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { if (it_edge->first != m_referenceCameraName) { it_img = mapOfImages.find(it_edge->first); it_camPose = mapOfCameraPoses.find(it_edge->first); - if (it_img != mapOfImages.end() && - it_camPose != mapOfCameraPoses.end()) { - it_edge->second->reInitModel(*it_img->second, cad_name, - it_camPose->second, verbose); + if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) { + it_edge->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose); } else { vectorOfMissingCameras.push_back(it_edge->first); } } } - for (std::vector::const_iterator it = - vectorOfMissingCameras.begin(); - it != vectorOfMissingCameras.end(); ++it) { + for (std::vector::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end(); + ++it) { it_img = mapOfImages.find(*it); std::map::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; - m_mapOfEdgeTrackers[*it]->reInitModel(*it_img->second, cad_name, - cCurrentMo, verbose); + m_mapOfEdgeTrackers[*it]->reInitModel(*it_img->second, cad_name, cCurrentMo, verbose); } } } @@ -2227,8 +1996,7 @@ void vpMbEdgeMultiTracker::resetTracker() this->cMo.eye(); // Reset all internal trackers - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->resetTracker(); } @@ -2269,8 +2037,7 @@ void vpMbEdgeMultiTracker::setAngleAppear(const double &a) { vpMbTracker::setAngleAppear(a); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setAngleAppear(a); } @@ -2289,8 +2056,7 @@ void vpMbEdgeMultiTracker::setAngleDisappear(const double &a) { vpMbTracker::setAngleDisappear(a); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setAngleDisappear(a); } @@ -2301,18 +2067,14 @@ void vpMbEdgeMultiTracker::setAngleDisappear(const double &a) \param camera : The new camera parameters. */ -void vpMbEdgeMultiTracker::setCameraParameters( - const vpCameraParameters &camera) +void vpMbEdgeMultiTracker::setCameraParameters(const vpCameraParameters &camera) { if (m_mapOfEdgeTrackers.empty()) { - throw vpException(vpTrackingException::fatalError, - "There is no camera !"); + throw vpException(vpTrackingException::fatalError, "There is no camera !"); } else if (m_mapOfEdgeTrackers.size() > 1) { - throw vpException(vpTrackingException::fatalError, - "There is more than one camera !"); + throw vpException(vpTrackingException::fatalError, "There is more than one camera !"); } else { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->setCameraParameters(camera); @@ -2320,8 +2082,7 @@ void vpMbEdgeMultiTracker::setCameraParameters( this->cam = camera; } else { std::stringstream ss; - ss << "The reference camera: " << m_referenceCameraName - << " does not exist !"; + ss << "The reference camera: " << m_referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } @@ -2335,16 +2096,13 @@ void vpMbEdgeMultiTracker::setCameraParameters( \param firstCameraIsReference : If true, the first camera is the reference, otherwise it is the second one. */ -void vpMbEdgeMultiTracker::setCameraParameters( - const vpCameraParameters &camera1, const vpCameraParameters &camera2, - const bool firstCameraIsReference) +void vpMbEdgeMultiTracker::setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2, + const bool firstCameraIsReference) { if (m_mapOfEdgeTrackers.empty()) { - throw vpException(vpTrackingException::fatalError, - "There is no camera !"); + throw vpException(vpTrackingException::fatalError, "There is no camera !"); } else if (m_mapOfEdgeTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it->second->setCameraParameters(camera1); ++it; @@ -2357,8 +2115,7 @@ void vpMbEdgeMultiTracker::setCameraParameters( } } else { std::stringstream ss; - ss << "Require two cameras ! There are " << m_mapOfEdgeTrackers.size() - << " cameras !"; + ss << "Require two cameras ! There are " << m_mapOfEdgeTrackers.size() << " cameras !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } @@ -2369,11 +2126,9 @@ void vpMbEdgeMultiTracker::setCameraParameters( \param cameraName : Camera name. \param camera : The new camera parameters. */ -void vpMbEdgeMultiTracker::setCameraParameters( - const std::string &cameraName, const vpCameraParameters &camera) +void vpMbEdgeMultiTracker::setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->setCameraParameters(camera); @@ -2392,14 +2147,11 @@ void vpMbEdgeMultiTracker::setCameraParameters( \param mapOfCameraParameters : Map of camera parameters. */ -void vpMbEdgeMultiTracker::setCameraParameters( - const std::map &mapOfCameraParameters) +void vpMbEdgeMultiTracker::setCameraParameters(const std::map &mapOfCameraParameters) { - for (std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { - std::map::const_iterator it_cam = - mapOfCameraParameters.find(it_edge->first); + std::map::const_iterator it_cam = mapOfCameraParameters.find(it_edge->first); if (it_cam != mapOfCameraParameters.end()) { it_edge->second->setCameraParameters(it_cam->second); @@ -2408,8 +2160,7 @@ void vpMbEdgeMultiTracker::setCameraParameters( } } else { std::stringstream ss; - ss << "Missing camera parameters for camera: " << it_edge->first - << " !"; + ss << "Missing camera parameters for camera: " << it_edge->first << " !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } @@ -2423,12 +2174,10 @@ void vpMbEdgeMultiTracker::setCameraParameters( \param cameraTransformationMatrix : Camera transformation matrix between the current and the reference camera. */ -void vpMbEdgeMultiTracker::setCameraTransformationMatrix( - const std::string &cameraName, - const vpHomogeneousMatrix &cameraTransformationMatrix) +void vpMbEdgeMultiTracker::setCameraTransformationMatrix(const std::string &cameraName, + const vpHomogeneousMatrix &cameraTransformationMatrix) { - std::map::iterator it = - m_mapOfCameraTransformationMatrix.find(cameraName); + std::map::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName); if (it != m_mapOfCameraTransformationMatrix.end()) { it->second = cameraTransformationMatrix; } else { @@ -2446,19 +2195,16 @@ void vpMbEdgeMultiTracker::setCameraTransformationMatrix( \param mapOfTransformationMatrix : map of camera transformation matrices. */ void vpMbEdgeMultiTracker::setCameraTransformationMatrix( - const std::map - &mapOfTransformationMatrix) + const std::map &mapOfTransformationMatrix) { // Check if all cameras have a transformation matrix - for (std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { std::map::const_iterator it_camTrans = mapOfTransformationMatrix.find(it_edge->first); if (it_camTrans == mapOfTransformationMatrix.end()) { - throw vpException(vpTrackingException::initializationError, - "Missing camera transformation matrix !"); + throw vpException(vpTrackingException::initializationError, "Missing camera transformation matrix !"); } } @@ -2477,8 +2223,7 @@ void vpMbEdgeMultiTracker::setClipping(const unsigned int &flags) // Set clipping for vpMbEdgeMultiTracker class vpMbTracker::setClipping(flags); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setClipping(flags); } @@ -2492,13 +2237,11 @@ void vpMbEdgeMultiTracker::setClipping(const unsigned int &flags) \param cameraName : Camera to set the clipping. \param flags : New clipping flags. */ -void vpMbEdgeMultiTracker::setClipping(const std::string &cameraName, - const unsigned int &flags) +void vpMbEdgeMultiTracker::setClipping(const std::string &cameraName, const unsigned int &flags) { // Set clipping for the given camera, do not change the clipping for // vpMbEdgeMultiTracker class - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->setClipping(flags); } else { @@ -2515,8 +2258,7 @@ void vpMbEdgeMultiTracker::setCovarianceComputation(const bool &flag) { vpMbTracker::setCovarianceComputation(flag); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setCovarianceComputation(flag); } @@ -2540,8 +2282,7 @@ void vpMbEdgeMultiTracker::setCovarianceComputation(const bool &flag) */ void vpMbEdgeMultiTracker::setDisplayFeatures(const bool displayF) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setDisplayFeatures(displayF); } @@ -2558,8 +2299,7 @@ void vpMbEdgeMultiTracker::setFarClippingDistance(const double &dist) { vpMbTracker::setFarClippingDistance(dist); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setFarClippingDistance(dist); } @@ -2571,11 +2311,9 @@ void vpMbEdgeMultiTracker::setFarClippingDistance(const double &dist) \param cameraName : Camera to set the far clipping. \param dist : Far clipping value. */ -void vpMbEdgeMultiTracker::setFarClippingDistance( - const std::string &cameraName, const double &dist) +void vpMbEdgeMultiTracker::setFarClippingDistance(const std::string &cameraName, const double &dist) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->setFarClippingDistance(dist); } else { @@ -2596,11 +2334,9 @@ void vpMbEdgeMultiTracker::setFarClippingDistance( \sa getGoodMovingEdgesRatioThreshold() */ -void vpMbEdgeMultiTracker::setGoodMovingEdgesRatioThreshold( - const double threshold) +void vpMbEdgeMultiTracker::setGoodMovingEdgesRatioThreshold(const double threshold) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setGoodMovingEdgesRatioThreshold(threshold); } @@ -2618,11 +2354,9 @@ void vpMbEdgeMultiTracker::setGoodMovingEdgesRatioThreshold( \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%). */ -void vpMbEdgeMultiTracker::setGoodNbRayCastingAttemptsRatio( - const double &ratio) +void vpMbEdgeMultiTracker::setGoodNbRayCastingAttemptsRatio(const double &ratio) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setGoodNbRayCastingAttemptsRatio(ratio); } @@ -2637,11 +2371,9 @@ void vpMbEdgeMultiTracker::setGoodNbRayCastingAttemptsRatio( \param attempts Number of rays to be sent. */ -void vpMbEdgeMultiTracker::setNbRayCastingAttemptsForVisibility( - const unsigned int &attempts) +void vpMbEdgeMultiTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setNbRayCastingAttemptsForVisibility(attempts); } @@ -2661,8 +2393,7 @@ void vpMbEdgeMultiTracker::setNbRayCastingAttemptsForVisibility( */ void vpMbEdgeMultiTracker::setLod(const bool useLod, const std::string &name) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setLod(useLod, name); } @@ -2680,18 +2411,14 @@ void vpMbEdgeMultiTracker::setLod(const bool useLod, const std::string &name) \sa setMinLineLengthThresh(), setMinPolygonAreaThresh() */ -void vpMbEdgeMultiTracker::setLod(const bool useLod, - const std::string &cameraName, - const std::string &name) +void vpMbEdgeMultiTracker::setLod(const bool useLod, const std::string &cameraName, const std::string &name) { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(cameraName); if (it_edge != m_mapOfEdgeTrackers.end()) { it_edge->second->setLod(useLod, name); } else { - std::cerr << "The camera: " << cameraName << " cannot be found !" - << std::endl; + std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl; } } @@ -2704,11 +2431,9 @@ void vpMbEdgeMultiTracker::setLod(const bool useLod, \sa setLod(), setMinPolygonAreaThresh() */ -void vpMbEdgeMultiTracker::setMinLineLengthThresh( - const double minLineLengthThresh, const std::string &name) +void vpMbEdgeMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setMinLineLengthThresh(minLineLengthThresh, name); } @@ -2725,18 +2450,15 @@ void vpMbEdgeMultiTracker::setMinLineLengthThresh( \sa setLod(), setMinPolygonAreaThresh() */ -void vpMbEdgeMultiTracker::setMinLineLengthThresh( - const double minLineLengthThresh, const std::string &cameraName, - const std::string &name) +void vpMbEdgeMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &cameraName, + const std::string &name) { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(cameraName); if (it_edge != m_mapOfEdgeTrackers.end()) { it_edge->second->setMinLineLengthThresh(minLineLengthThresh, name); } else { - std::cerr << "The camera: " << cameraName << " cannot be found !" - << std::endl; + std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl; } } @@ -2748,11 +2470,9 @@ void vpMbEdgeMultiTracker::setMinLineLengthThresh( \sa setLod(), setMinLineLengthThresh() */ -void vpMbEdgeMultiTracker::setMinPolygonAreaThresh( - const double minPolygonAreaThresh, const std::string &name) +void vpMbEdgeMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name); } @@ -2768,18 +2488,15 @@ void vpMbEdgeMultiTracker::setMinPolygonAreaThresh( \sa setLod(), setMinLineLengthThresh() */ -void vpMbEdgeMultiTracker::setMinPolygonAreaThresh( - const double minPolygonAreaThresh, const std::string &cameraName, - const std::string &name) +void vpMbEdgeMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName, + const std::string &name) { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(cameraName); if (it_edge != m_mapOfEdgeTrackers.end()) { it_edge->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name); } else { - std::cerr << "The camera: " << cameraName << " cannot be found !" - << std::endl; + std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl; } } @@ -2790,8 +2507,7 @@ void vpMbEdgeMultiTracker::setMinPolygonAreaThresh( */ void vpMbEdgeMultiTracker::setMovingEdge(const vpMe &me) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setMovingEdge(me); } @@ -2803,11 +2519,9 @@ void vpMbEdgeMultiTracker::setMovingEdge(const vpMe &me) \param cameraName : Camera name to set the moving edge parameters. \param me : An instance of vpMe containing all the desired parameters. */ -void vpMbEdgeMultiTracker::setMovingEdge(const std::string &cameraName, - const vpMe &me) +void vpMbEdgeMultiTracker::setMovingEdge(const std::string &cameraName, const vpMe &me) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->setMovingEdge(me); } else { @@ -2824,8 +2538,7 @@ void vpMbEdgeMultiTracker::setNearClippingDistance(const double &dist) { vpMbTracker::setNearClippingDistance(dist); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setNearClippingDistance(dist); } @@ -2837,11 +2550,9 @@ void vpMbEdgeMultiTracker::setNearClippingDistance(const double &dist) \param cameraName : Camera name to set the near clipping distance. \param dist : Near clipping value. */ -void vpMbEdgeMultiTracker::setNearClippingDistance( - const std::string &cameraName, const double &dist) +void vpMbEdgeMultiTracker::setNearClippingDistance(const std::string &cameraName, const double &dist) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(cameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->setNearClippingDistance(dist); } else { @@ -2859,11 +2570,9 @@ void vpMbEdgeMultiTracker::setNearClippingDistance( Ogre rendering options) when Ogre visibility is enabled. By default, this functionality is turned off. */ -void vpMbEdgeMultiTracker::setOgreShowConfigDialog( - const bool showConfigDialog) +void vpMbEdgeMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setOgreShowConfigDialog(showConfigDialog); } @@ -2879,18 +2588,15 @@ void vpMbEdgeMultiTracker::setOgreShowConfigDialog( */ void vpMbEdgeMultiTracker::setOgreVisibilityTest(const bool &v) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setOgreVisibilityTest(v); } #ifdef VISP_HAVE_OGRE - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { - it->second->faces.getOgreContext()->setWindowName("Multi MBT Edge (" + - it->first + ")"); + it->second->faces.getOgreContext()->setWindowName("Multi MBT Edge (" + it->first + ")"); } #endif @@ -2902,11 +2608,9 @@ void vpMbEdgeMultiTracker::setOgreVisibilityTest(const bool &v) \param opt : Optimization method to use. */ -void vpMbEdgeMultiTracker::setOptimizationMethod( - const vpMbtOptimizationMethod &opt) +void vpMbEdgeMultiTracker::setOptimizationMethod(const vpMbtOptimizationMethod &opt) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setOptimizationMethod(opt); } @@ -2921,19 +2625,16 @@ void vpMbEdgeMultiTracker::setOptimizationMethod( \param I : image corresponding to the desired pose. \param cMo_ : Pose to affect. */ -void vpMbEdgeMultiTracker::setPose(const vpImage &I, - const vpHomogeneousMatrix &cMo_) +void vpMbEdgeMultiTracker::setPose(const vpImage &I, const vpHomogeneousMatrix &cMo_) { if (m_mapOfEdgeTrackers.size() == 1) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->setPose(I, cMo_); this->cMo = cMo_; } else { std::stringstream ss; - ss << "Cannot find the reference camera: " << m_referenceCameraName - << " !"; + ss << "Cannot find the reference camera: " << m_referenceCameraName << " !"; throw vpException(vpTrackingException::fatalError, ss.str()); } } else { @@ -2955,15 +2656,12 @@ void vpMbEdgeMultiTracker::setPose(const vpImage &I, \param firstCameraIsReference : if true, the first camera is the reference, otherwise it is the second one. */ -void vpMbEdgeMultiTracker::setPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, +void vpMbEdgeMultiTracker::setPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference) { if (m_mapOfEdgeTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it->second->setPose(I1, c1Mo); ++it; @@ -2977,8 +2675,7 @@ void vpMbEdgeMultiTracker::setPose(const vpImage &I1, } } else { std::stringstream ss; - ss << "This method requires 2 cameras but there are " - << m_mapOfEdgeTrackers.size() << " cameras !"; + ss << "This method requires 2 cameras but there are " << m_mapOfEdgeTrackers.size() << " cameras !"; throw vpException(vpTrackingException::fatalError, ss.str()); } } @@ -2991,15 +2688,13 @@ void vpMbEdgeMultiTracker::setPose(const vpImage &I1, \param mapOfImages : Map of images. \param cMo_ : Pose to affect to the reference camera. */ -void vpMbEdgeMultiTracker::setPose( - const std::map *> &mapOfImages, - const vpHomogeneousMatrix &cMo_) +void vpMbEdgeMultiTracker::setPose(const std::map *> &mapOfImages, + const vpHomogeneousMatrix &cMo_) { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it_edge != m_mapOfEdgeTrackers.end()) { - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); if (it_img != mapOfImages.end()) { // Set pose on reference camera @@ -3009,22 +2704,17 @@ void vpMbEdgeMultiTracker::setPose( cMo = cMo_; // Set the pose for the others cameras - for (it_edge = m_mapOfEdgeTrackers.begin(); - it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { + for (it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { if (it_edge->first != m_referenceCameraName) { it_img = mapOfImages.find(it_edge->first); - std::map::const_iterator - it_camTrans = - m_mapOfCameraTransformationMatrix.find(it_edge->first); + std::map::const_iterator it_camTrans = + m_mapOfCameraTransformationMatrix.find(it_edge->first); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; it_edge->second->setPose(*it_img->second, cCurrentMo); } else { - throw vpException( - vpTrackingException::fatalError, - "Missing image or camera transformation matrix !"); + throw vpException(vpTrackingException::fatalError, "Missing image or camera transformation matrix !"); } } } @@ -3035,8 +2725,7 @@ void vpMbEdgeMultiTracker::setPose( } } else { std::stringstream ss; - ss << "The reference camera: " << m_referenceCameraName - << " does not exist !"; + ss << "The reference camera: " << m_referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::fatalError, ss.str()); } } @@ -3051,39 +2740,32 @@ void vpMbEdgeMultiTracker::setPose( \param mapOfImages : Map of images corresponding to the desired pose. \param mapOfCameraPoses : Map of poses to affect. */ -void vpMbEdgeMultiTracker::setPose( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses) +void vpMbEdgeMultiTracker::setPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses) { // Set the reference cMo - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(m_referenceCameraName); - - if (it_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && - it_camPose != mapOfCameraPoses.end()) { + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName); + + if (it_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) { it_edge->second->setPose(*it_img->second, it_camPose->second); it_edge->second->getPose(cMo); } else { - throw vpException(vpTrackingException::fatalError, - "Cannot set pose for the reference camera !"); + throw vpException(vpTrackingException::fatalError, "Cannot set pose for the reference camera !"); } // Vector of missing pose matrices for cameras std::vector vectorOfMissingCameraPoses; // Set pose for the specified cameras - for (it_edge = m_mapOfEdgeTrackers.begin(); - it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { + for (it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { if (it_edge->first != m_referenceCameraName) { it_img = mapOfImages.find(it_edge->first); it_camPose = mapOfCameraPoses.find(it_edge->first); - if (it_img != mapOfImages.end() && - it_camPose != mapOfCameraPoses.end()) { + if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) { // Set pose it_edge->second->setPose(*it_img->second, it_camPose->second); } else { @@ -3092,15 +2774,13 @@ void vpMbEdgeMultiTracker::setPose( } } - for (std::vector::const_iterator it1 = - vectorOfMissingCameraPoses.begin(); + for (std::vector::const_iterator it1 = vectorOfMissingCameraPoses.begin(); it1 != vectorOfMissingCameraPoses.end(); ++it1) { it_img = mapOfImages.find(*it1); std::map::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it1); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; m_mapOfEdgeTrackers[*it1]->setPose(*it_img->second, cCurrentMo); } else { @@ -3125,8 +2805,7 @@ void vpMbEdgeMultiTracker::setProjectionErrorComputation(const bool &flag) vpMbTracker::setProjectionErrorComputation(flag); // Set the flag for each camera - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setProjectionErrorComputation(flag); } @@ -3137,17 +2816,14 @@ void vpMbEdgeMultiTracker::setProjectionErrorComputation(const bool &flag) \param referenceCameraName : Name of the reference camera. */ -void vpMbEdgeMultiTracker::setReferenceCameraName( - const std::string &referenceCameraName) +void vpMbEdgeMultiTracker::setReferenceCameraName(const std::string &referenceCameraName) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(referenceCameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { m_referenceCameraName = referenceCameraName; } else { std::stringstream ss; - ss << "The reference camera: " << referenceCameraName - << " does not exist !"; + ss << "The reference camera: " << referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::fatalError, ss.str()); } } @@ -3176,8 +2852,7 @@ void vpMbEdgeMultiTracker::setReferenceCameraName( void vpMbEdgeMultiTracker::setScales(const std::vector &scale) { vpMbEdgeTracker::setScales(scale); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setScales(scale); } @@ -3193,8 +2868,7 @@ void vpMbEdgeMultiTracker::setScanLineVisibilityTest(const bool &v) // Set general setScanLineVisibilityTest vpMbTracker::setScanLineVisibilityTest(v); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setScanLineVisibilityTest(v); } @@ -3207,11 +2881,9 @@ void vpMbEdgeMultiTracker::setScanLineVisibilityTest(const bool &v) \param name : name of the polygon(s). \param useEdgeTracking : True if it has to be considered, false otherwise. */ -void vpMbEdgeMultiTracker::setUseEdgeTracking(const std::string &name, - const bool &useEdgeTracking) +void vpMbEdgeMultiTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setUseEdgeTracking(name, useEdgeTracking); } @@ -3228,16 +2900,14 @@ void vpMbEdgeMultiTracker::track(const vpImage &I) { // Track only with reference camera // Get the reference camera parameters - std::map::const_iterator it = - m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName); if (it != m_mapOfEdgeTrackers.end()) { it->second->track(I); it->second->getPose(cMo); } else { std::stringstream ss; - ss << "The reference camera: " << m_referenceCameraName - << " does not exist !"; + ss << "The reference camera: " << m_referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::fatalError, ss.str()); } @@ -3256,13 +2926,11 @@ void vpMbEdgeMultiTracker::track(const vpImage &I) \param I1 : The first image. \param I2 : The second image. */ -void vpMbEdgeMultiTracker::track(const vpImage &I1, - const vpImage &I2) +void vpMbEdgeMultiTracker::track(const vpImage &I1, const vpImage &I2) { // Track with the special case of stereo cameras if (m_mapOfEdgeTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); std::map *> mapOfImages; mapOfImages[it->first] = &I1; ++it; @@ -3271,8 +2939,7 @@ void vpMbEdgeMultiTracker::track(const vpImage &I1, track(mapOfImages); } else { std::stringstream ss; - ss << "Require two cameras ! There are " << m_mapOfEdgeTrackers.size() - << " cameras !"; + ss << "Require two cameras ! There are " << m_mapOfEdgeTrackers.size() << " cameras !"; throw vpException(vpTrackingException::fatalError, ss.str()); } } @@ -3284,15 +2951,12 @@ void vpMbEdgeMultiTracker::track(const vpImage &I1, \param mapOfImages : Map of images. */ -void vpMbEdgeMultiTracker::track( - std::map *> &mapOfImages) +void vpMbEdgeMultiTracker::track(std::map *> &mapOfImages) { // Check if there is an image for each camera - for (std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { - std::map *>::const_iterator - it_img = mapOfImages.find(it_edge->first); + std::map *>::const_iterator it_img = mapOfImages.find(it_edge->first); if (it_img == mapOfImages.end()) { throw vpException(vpTrackingException::fatalError, "Missing images !"); @@ -3311,16 +2975,14 @@ void vpMbEdgeMultiTracker::track( vpHomogeneousMatrix cMo_1 = cMo; try { downScale(lvl); - for (std::map::const_iterator it1 = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it1 = m_mapOfEdgeTrackers.begin(); it1 != m_mapOfEdgeTrackers.end(); ++it1) { // Downscale for each camera it1->second->downScale(lvl); // Track moving edges try { - it1->second->trackMovingEdge( - *m_mapOfPyramidalImages[it1->first][lvl]); + it1->second->trackMovingEdge(*m_mapOfPyramidalImages[it1->first][lvl]); } catch (...) { vpTRACE("Error in moving edge tracking"); throw; @@ -3328,10 +2990,8 @@ void vpMbEdgeMultiTracker::track( } try { - std::map *> - mapOfPyramidImages; - for (std::map - *> >::const_iterator it = + std::map *> mapOfPyramidImages; + for (std::map *> >::const_iterator it = m_mapOfPyramidalImages.begin(); it != m_mapOfPyramidalImages.end(); ++it) { mapOfPyramidImages[it->first] = it->second[lvl]; @@ -3345,12 +3005,10 @@ void vpMbEdgeMultiTracker::track( // Test tracking failed only if all testTracking failed bool isOneTestTrackingOk = false; - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { // Set the camera pose - it->second->cMo = - m_mapOfCameraTransformationMatrix[it->first] * cMo; + it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo; try { it->second->testTracking(); @@ -3364,15 +3022,12 @@ void vpMbEdgeMultiTracker::track( std::ostringstream oss; oss << "Not enough moving edges to track the object. Try to reduce " "the threshold=" - << percentageGdPt - << " using setGoodMovingEdgesRatioThreshold()"; - throw vpTrackingException(vpTrackingException::fatalError, - oss.str()); + << percentageGdPt << " using setGoodMovingEdgesRatioThreshold()"; + throw vpTrackingException(vpTrackingException::fatalError, oss.str()); } if (displayFeatures) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->displayFeaturesOnImage(*mapOfImages[it->first], lvl); } @@ -3380,28 +3035,22 @@ void vpMbEdgeMultiTracker::track( // Looking for new visible face bool newvisibleface = false; - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { - it->second->visibleFace(*mapOfImages[it->first], it->second->cMo, - newvisibleface); + it->second->visibleFace(*mapOfImages[it->first], it->second->cMo, newvisibleface); } if (useScanLine) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { - it->second->faces.computeClippedPolygons(it->second->cMo, - it->second->cam); - it->second->faces.computeScanLineRender( - it->second->cam, mapOfImages[it->first]->getWidth(), - mapOfImages[it->first]->getHeight()); + it->second->faces.computeClippedPolygons(it->second->cMo, it->second->cam); + it->second->faces.computeScanLineRender(it->second->cam, mapOfImages[it->first]->getWidth(), + mapOfImages[it->first]->getHeight()); } } try { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->updateMovingEdge(*mapOfImages[it->first]); } @@ -3409,15 +3058,12 @@ void vpMbEdgeMultiTracker::track( throw; // throw the original exception } - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { - it->second->initMovingEdge(*mapOfImages[it->first], - it->second->cMo); + it->second->initMovingEdge(*mapOfImages[it->first], it->second->cMo); // Reinit the moving edge for the lines which need it. - it->second->reinitMovingEdge(*mapOfImages[it->first], - it->second->cMo); + it->second->reinitMovingEdge(*mapOfImages[it->first], it->second->cMo); if (computeProjError) { // Compute the projection error @@ -3428,8 +3074,7 @@ void vpMbEdgeMultiTracker::track( computeProjectionError(); upScale(lvl); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->upScale(lvl); } @@ -3439,8 +3084,7 @@ void vpMbEdgeMultiTracker::track( reInitLevel(lvl); upScale(lvl); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->cMo = cMo_1; it->second->reInitLevel(lvl); @@ -3448,8 +3092,7 @@ void vpMbEdgeMultiTracker::track( } } else { upScale(lvl); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->upScale(lvl); } diff --git a/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp b/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp index 5d5adc11e2..fa9e351982 100644 --- a/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp +++ b/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp @@ -67,13 +67,11 @@ Basic constructor */ vpMbEdgeTracker::vpMbEdgeTracker() - : me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), - ncylinder(0), nbvisiblepolygone(0), percentageGdPt(0.4), scales(1), - Ipyramid(0), scaleLevel(0), nbFeaturesForProjErrorComputation(0), - m_factor(), m_robustLines(), m_robustCylinders(), m_robustCircles(), - m_wLines(), m_wCylinders(), m_wCircles(), m_errorLines(), - m_errorCylinders(), m_errorCircles(), m_L_edge(), m_error_edge(), - m_w_edge(), m_weightedError_edge(), m_robust_edge() + : me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0), nbvisiblepolygone(0), + percentageGdPt(0.4), scales(1), Ipyramid(0), scaleLevel(0), nbFeaturesForProjErrorComputation(0), m_factor(), + m_robustLines(), m_robustCylinders(), m_robustCircles(), m_wLines(), m_wCylinders(), m_wCircles(), m_errorLines(), + m_errorCylinders(), m_errorCircles(), m_L_edge(), m_error_edge(), m_w_edge(), m_weightedError_edge(), + m_robust_edge() { angleAppears = vpMath::rad(89); angleDisappears = vpMath::rad(89); @@ -96,9 +94,7 @@ vpMbEdgeTracker::~vpMbEdgeTracker() for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) { l = *it; if (l != NULL) { delete l; @@ -106,9 +102,8 @@ vpMbEdgeTracker::~vpMbEdgeTracker() l = NULL; } - for (std::list::const_iterator it = - cylinders[i].begin(); - it != cylinders[i].end(); ++it) { + for (std::list::const_iterator it = cylinders[i].begin(); it != cylinders[i].end(); + ++it) { cy = *it; if (cy != NULL) { delete cy; @@ -121,9 +116,7 @@ vpMbEdgeTracker::~vpMbEdgeTracker() } } for (unsigned int i = 0; i < circles.size(); i += 1) { - for (std::list::const_iterator it = - circles[i].begin(); - it != circles[i].end(); ++it) { + for (std::list::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) { ci = *it; if (ci != NULL) { delete ci; @@ -146,23 +139,18 @@ void vpMbEdgeTracker::setMovingEdge(const vpMe &p_me) for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) { vpMbtDistanceLine *l = *it; l->setMovingEdge(&(this->me)); } - for (std::list::const_iterator it = - cylinders[i].begin(); - it != cylinders[i].end(); ++it) { + for (std::list::const_iterator it = cylinders[i].begin(); it != cylinders[i].end(); + ++it) { vpMbtDistanceCylinder *cy = *it; cy->setMovingEdge(&(this->me)); } - for (std::list::const_iterator it = - circles[i].begin(); - it != circles[i].end(); ++it) { + for (std::list::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) { vpMbtDistanceCircle *ci = *it; ci->setMovingEdge(&(this->me)); } @@ -179,8 +167,7 @@ void vpMbEdgeTracker::setMovingEdge(const vpMe &p_me) \param _I : The current image. \param lvl : The level in the pyramid scale. */ -void vpMbEdgeTracker::computeVVS(const vpImage &_I, - const unsigned int lvl) +void vpMbEdgeTracker::computeVVS(const vpImage &_I, const unsigned int lvl) { double residu_1 = 1e3; double r = 1e3 - 1; @@ -192,8 +179,7 @@ void vpMbEdgeTracker::computeVVS(const vpImage &_I, bool reloop = true; - bool isoJoIdentity_ = - isoJoIdentity; // Backup since it can be modified if L is not full rank + bool isoJoIdentity_ = isoJoIdentity; // Backup since it can be modified if L is not full rank if (isoJoIdentity_) oJo.eye(); @@ -236,14 +222,11 @@ void vpMbEdgeTracker::computeVVS(const vpImage &_I, m_w_edge = 1; // while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30)) - while (std::fabs((residu_1 - r) * 1e8) > - std::numeric_limits::epsilon() && - (iter < m_maxIter)) { + while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits::epsilon() && (iter < m_maxIter)) { computeVVSInteractionMatrixAndResidu(_I); bool reStartFromLastIncrement = false; - computeVVSCheckLevenbergMarquardt(iter, m_error_edge, m_error_prev, - cMoPrev, mu, reStartFromLastIncrement, + computeVVSCheckLevenbergMarquardt(iter, m_error_edge, m_error_prev, cMoPrev, mu, reStartFromLastIncrement, &m_w_edge, &m_w_prev); if (!reStartFromLastIncrement) { @@ -291,9 +274,8 @@ void vpMbEdgeTracker::computeVVS(const vpImage &_I, residu_1 = r; r = sqrt(num / den); // Le critere d'arret prend en compte le poids - computeVVSPoseEstimation( - isoJoIdentity_, iter, m_L_edge, LTL, m_weightedError_edge, - m_error_edge, m_error_prev, LTR, mu, v, &m_w_edge, &m_w_prev); + computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_edge, LTL, m_weightedError_edge, m_error_edge, m_error_prev, + LTR, mu, v, &m_w_edge, &m_w_prev); cMoPrev = cMo; cMo = vpExponentialMap::direct(v).inverse() * cMo; @@ -303,41 +285,35 @@ void vpMbEdgeTracker::computeVVS(const vpImage &_I, iter++; } - computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMoPrev, L_true, - LVJ_true, m_error_edge); + computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMoPrev, L_true, LVJ_true, m_error_edge); updateMovingEdgeWeights(); } -void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, - const unsigned int iter, - double &count, +void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, const unsigned int iter, double &count, const unsigned int lvl) { vpMbtDistanceLine *l; vpMbtDistanceCylinder *cy; vpMbtDistanceCircle *ci; - double limite = 3; // Une limite de 3 pixels - limite = - limite / cam.get_px(); // Transformation limite pixel en limite metre. + double limite = 3; // Une limite de 3 pixels + limite = limite / cam.get_px(); // Transformation limite pixel en limite metre. unsigned int n = 0; // Parametre pour la premiere phase d'asservissement double e_prev = 0, e_cur, e_next; - for (std::list::const_iterator it = lines[lvl].begin(); - it != lines[lvl].end(); ++it) { + for (std::list::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) { if ((*it)->isTracked()) { l = *it; l->computeInteractionMatrixError(cMo); double fac = 1; if (iter == 0) { - for (std::list::const_iterator itindex = - l->Lindex_polygon.begin(); - itindex != l->Lindex_polygon.end(); ++itindex) { + for (std::list::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; @@ -360,12 +336,9 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, for (unsigned int i = 0; i < l->nbFeature[a]; i++) { for (unsigned int j = 0; j < 6; j++) { - m_L_edge[n + i][j] = - l->L[indexFeature] - [j]; // On remplit la matrice d'interaction globale + m_L_edge[n + i][j] = l->L[indexFeature][j]; // On remplit la matrice d'interaction globale } - m_error_edge[n + i] = - l->error[indexFeature]; // On remplit la matrice d'erreur + m_error_edge[n + i] = l->error[indexFeature]; // On remplit la matrice d'erreur if (m_error_edge[n + i] <= limite) count = count + 1.0; // Si erreur proche de 0 on incremente cur @@ -385,8 +358,7 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, 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)) { + if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) { m_w_edge[n + i] = 1 /*0.5*/; } e_prev = e_cur; @@ -397,8 +369,7 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, // 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)) { + if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) { m_w_edge[n + i] += 1 /*0.5*/; } } @@ -421,9 +392,8 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, } } - for (std::list::const_iterator it = - cylinders[lvl].begin(); - it != cylinders[lvl].end(); ++it) { + for (std::list::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end(); + ++it) { if ((*it)->isTracked()) { cy = *it; cy->computeInteractionMatrixError(cMo, _I); @@ -438,8 +408,7 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, for (unsigned int i = 0; i < cy->nbFeature; i++) { for (unsigned int j = 0; j < 6; j++) { - m_L_edge[n + i][j] = - cy->L[i][j]; // On remplit la matrice d'interaction globale + m_L_edge[n + i][j] = cy->L[i][j]; // On remplit la matrice d'interaction globale } m_error_edge[n + i] = cy->error[i]; // On remplit la matrice d'erreur @@ -467,8 +436,7 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, 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)) { + if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) { m_w_edge[n + i] = 1 /*0.5*/; } e_prev = e_cur; @@ -479,8 +447,7 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, 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)) { + if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) { m_w_edge[n + i] = 1 /*0.5*/; } e_prev = e_cur; @@ -491,16 +458,14 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, // 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)) { + if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) { m_w_edge[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)) { + if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) { m_w_edge[n + i] += 1 /*0.5*/; } } @@ -522,9 +487,7 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, } } - for (std::list::const_iterator it = - circles[lvl].begin(); - it != circles[lvl].end(); ++it) { + for (std::list::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) { if ((*it)->isTracked()) { ci = *it; ci->computeInteractionMatrixError(cMo); @@ -537,8 +500,7 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, for (unsigned int i = 0; i < ci->nbFeature; i++) { for (unsigned int j = 0; j < 6; j++) { - m_L_edge[n + i][j] = - ci->L[i][j]; // On remplit la matrice d'interaction globale + m_L_edge[n + i][j] = ci->L[i][j]; // On remplit la matrice d'interaction globale } m_error_edge[n + i] = ci->error[i]; // On remplit la matrice d'erreur @@ -560,8 +522,7 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, 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)) { + if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) { m_w_edge[n + i] = 1 /*0.5*/; } e_prev = e_cur; @@ -572,8 +533,7 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, // 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)) { + if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) { m_w_edge[n + i] += 1 /*0.5*/; } } @@ -596,23 +556,21 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, } } -void vpMbEdgeTracker::computeVVSFirstPhaseFactor( - const vpImage &I, const unsigned int lvl) +void vpMbEdgeTracker::computeVVSFirstPhaseFactor(const vpImage &I, const unsigned int lvl) { vpMbtDistanceLine *l; vpMbtDistanceCylinder *cy; vpMbtDistanceCircle *ci; unsigned int n = 0; - for (std::list::const_iterator it = lines[lvl].begin(); - it != lines[lvl].end(); ++it) { + for (std::list::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::const_iterator itindex = l->Lindex_polygon.begin(); - itindex != l->Lindex_polygon.end(); ++itindex) { + for (std::list::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; @@ -644,9 +602,8 @@ void vpMbEdgeTracker::computeVVSFirstPhaseFactor( } } - for (std::list::const_iterator it = - cylinders[lvl].begin(); - it != cylinders[lvl].end(); ++it) { + for (std::list::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end(); + ++it) { if ((*it)->isTracked()) { cy = *it; cy->computeInteractionMatrixError(cMo, I); @@ -676,9 +633,7 @@ void vpMbEdgeTracker::computeVVSFirstPhaseFactor( } } - for (std::list::const_iterator it = - circles[lvl].begin(); - it != circles[lvl].end(); ++it) { + for (std::list::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) { if ((*it)->isTracked()) { ci = *it; ci->computeInteractionMatrixError(cMo); @@ -701,8 +656,7 @@ void vpMbEdgeTracker::computeVVSFirstPhaseFactor( } } -void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation( - const unsigned int iter, bool &isoJoIdentity_) +void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_) { unsigned int nerror = m_weightedError_edge.getRows(); @@ -739,8 +693,7 @@ void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation( vpMatrix K; // kernel unsigned int rank = (m_L_edge * cVo).kernel(K); if (rank == 0) { - throw vpException(vpException::fatalError, - "Rank=0, cannot estimate the pose !"); + throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !"); } if (rank != 6) { vpMatrix I; // Identity @@ -758,20 +711,14 @@ void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation( if (isoJoIdentity_) { LTL = m_L_edge.AtA(); computeJTR(m_L_edge, m_weightedError_edge, LTR); - v = -0.7 * - LTL.pseudoInverse(LTL.getRows() * - std::numeric_limits::epsilon()) * - LTR; + v = -0.7 * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits::epsilon()) * LTR; } else { cVo.buildFrom(cMo); vpMatrix LVJ = (m_L_edge * cVo * oJo); vpMatrix LVJTLVJ = (LVJ).AtA(); vpColVector LVJTR; computeJTR(LVJ, m_weightedError_edge, LVJTR); - v = -0.7 * - LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * - std::numeric_limits::epsilon()) * - LVJTR; + v = -0.7 * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits::epsilon()) * LVJTR; v = cVo * v; } @@ -786,13 +733,11 @@ void vpMbEdgeTracker::computeVVSInit() unsigned int nberrors_cylinders = 0; unsigned int nberrors_circles = 0; - nbrow = - initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles); + nbrow = initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles); if (nbrow == 0) { - throw vpTrackingException( - vpTrackingException::notEnoughPointError, - "No data found to compute the interaction matrix..."); + throw vpTrackingException(vpTrackingException::notEnoughPointError, + "No data found to compute the interaction matrix..."); } m_L_edge.resize(nbrow, 6, false, false); @@ -830,8 +775,7 @@ void vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu() "esidu() should not be called!"); } -void vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu( - const vpImage &_I) +void vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu(const vpImage &_I) { vpMbtDistanceLine *l; vpMbtDistanceCylinder *cy; @@ -842,9 +786,8 @@ void vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu( unsigned int ncylinders = 0; unsigned int ncircles = 0; - for (std::list::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { if ((*it)->isTracked()) { l = *it; l->computeInteractionMatrixError(cMo); @@ -860,8 +803,7 @@ void vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu( } } - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { if ((*it)->isTracked()) { cy = *it; @@ -879,8 +821,7 @@ void vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu( } } - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { if ((*it)->isTracked()) { ci = *it; @@ -901,15 +842,13 @@ void vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu( void vpMbEdgeTracker::computeVVSWeights() { - unsigned int nberrors_lines = m_errorLines.getRows(), - nberrors_cylinders = m_errorCylinders.getRows(), + unsigned int nberrors_lines = m_errorLines.getRows(), nberrors_cylinders = m_errorCylinders.getRows(), nberrors_circles = m_errorCircles.getRows(); if (nberrors_lines > 0) m_robustLines.MEstimator(vpRobust::TUKEY, m_errorLines, m_wLines); if (nberrors_cylinders > 0) - m_robustCylinders.MEstimator(vpRobust::TUKEY, m_errorCylinders, - m_wCylinders); + m_robustCylinders.MEstimator(vpRobust::TUKEY, m_errorCylinders, m_wCylinders); if (nberrors_circles > 0) m_robustCircles.MEstimator(vpRobust::TUKEY, m_errorCircles, m_wCircles); @@ -930,17 +869,15 @@ void vpMbEdgeTracker::computeProjectionError(const vpImage &_I) { projectionError = 0.0; unsigned int nbFeatures = 0; - for (std::list::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { vpMbtDistanceLine *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); + l->meline[a]->computeProjectionError(_I, lineNormGradient, lineNbFeatures); projectionError += lineNormGradient; nbFeatures += lineNbFeatures; } @@ -948,16 +885,14 @@ void vpMbEdgeTracker::computeProjectionError(const vpImage &_I) } } - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { vpMbtDistanceCylinder *cy = *it; if (cy->isVisible() && cy->isTracked()) { if (cy->meline1 != NULL) { double cylinderNormGradient = 0; unsigned int cylinderNbFeatures = 0; - cy->meline1->computeProjectionError(_I, cylinderNormGradient, - cylinderNbFeatures); + cy->meline1->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures); projectionError += cylinderNormGradient; nbFeatures += cylinderNbFeatures; } @@ -965,23 +900,20 @@ void vpMbEdgeTracker::computeProjectionError(const vpImage &_I) if (cy->meline2 != NULL) { double cylinderNormGradient = 0; unsigned int cylinderNbFeatures = 0; - cy->meline2->computeProjectionError(_I, cylinderNormGradient, - cylinderNbFeatures); + cy->meline2->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures); projectionError += cylinderNormGradient; nbFeatures += cylinderNbFeatures; } } } - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { vpMbtDistanceCircle *c = *it; if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) { double circleNormGradient = 0; unsigned int circleNbFeatures = 0; - c->meEllipse->computeProjectionError(_I, circleNormGradient, - circleNbFeatures); + c->meEllipse->computeProjectionError(_I, circleNormGradient, circleNbFeatures); projectionError += circleNormGradient; nbFeatures += circleNbFeatures; } @@ -1008,16 +940,14 @@ void vpMbEdgeTracker::testTracking() int nbGoodPoint = 0; int nbBadPoint = 0; - for (std::list::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { vpMbtDistanceLine *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::const_iterator itme = - l->meline[a]->getMeList().begin(); + for (std::list::const_iterator itme = l->meline[a]->getMeList().begin(); itme != l->meline[a]->getMeList().end(); ++itme) { vpMeSite pix = *itme; if (pix.getState() == vpMeSite::NO_SUPPRESSION) @@ -1030,15 +960,12 @@ void vpMbEdgeTracker::testTracking() } } - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { vpMbtDistanceCylinder *cy = *it; - if ((cy->meline1 != NULL && cy->meline2 != NULL) && cy->isVisible() && - cy->isTracked()) { + if ((cy->meline1 != NULL && cy->meline2 != NULL) && cy->isVisible() && cy->isTracked()) { nbExpectedPoint += (int)cy->meline1->expecteddensity; - for (std::list::const_iterator itme1 = - cy->meline1->getMeList().begin(); + for (std::list::const_iterator itme1 = cy->meline1->getMeList().begin(); itme1 != cy->meline1->getMeList().end(); ++itme1) { vpMeSite pix = *itme1; if (pix.getState() == vpMeSite::NO_SUPPRESSION) @@ -1047,8 +974,7 @@ void vpMbEdgeTracker::testTracking() nbBadPoint++; } nbExpectedPoint += (int)cy->meline2->expecteddensity; - for (std::list::const_iterator itme2 = - cy->meline2->getMeList().begin(); + for (std::list::const_iterator itme2 = cy->meline2->getMeList().begin(); itme2 != cy->meline2->getMeList().end(); ++itme2) { vpMeSite pix = *itme2; if (pix.getState() == vpMeSite::NO_SUPPRESSION) @@ -1059,14 +985,12 @@ void vpMbEdgeTracker::testTracking() } } - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { vpMbtDistanceCircle *ci = *it; if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) { nbExpectedPoint += ci->meEllipse->getExpectedDensity(); - for (std::list::const_iterator itme = - ci->meEllipse->getMeList().begin(); + for (std::list::const_iterator itme = ci->meEllipse->getMeList().begin(); itme != ci->meEllipse->getMeList().end(); ++itme) { vpMeSite pix = *itme; if (pix.getState() == vpMeSite::NO_SUPPRESSION) @@ -1079,14 +1003,11 @@ void vpMbEdgeTracker::testTracking() // 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 = (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 + 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()); @@ -1259,8 +1180,7 @@ void vpMbEdgeTracker::init(const vpImage &I) \param I : image corresponding to the desired pose. \param cdMo : Pose to affect. */ -void vpMbEdgeTracker::setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo) +void vpMbEdgeTracker::setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo) { cMo = cdMo; @@ -1346,12 +1266,10 @@ void vpMbEdgeTracker::loadConfigFile(const char *configFile) xmlp.setMovingEdge(me); try { - std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " - << std::endl; + std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl; xmlp.parse(configFile); } catch (...) { - throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", - configFile); + throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile); } vpCameraParameters camera; @@ -1401,29 +1319,23 @@ void vpMbEdgeTracker::loadConfigFile(const char *configFile) \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, +void vpMbEdgeTracker::display(const vpImage &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::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } @@ -1449,29 +1361,23 @@ void vpMbEdgeTracker::display(const vpImage &I, \param displayFullModel : If true, the full model is displayed (even the non visible surfaces). */ -void vpMbEdgeTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, +void vpMbEdgeTracker::display(const vpImage &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::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } @@ -1485,31 +1391,25 @@ void vpMbEdgeTracker::display(const vpImage &I, #endif } -void vpMbEdgeTracker::displayFeaturesOnImage(const vpImage &I, - const unsigned int lvl) +void vpMbEdgeTracker::displayFeaturesOnImage(const vpImage &I, const unsigned int lvl) { if (lvl == 0) { - for (std::list::const_iterator it = - lines[lvl].begin(); - it != lines[lvl].end(); ++it) { + for (std::list::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) { vpMbtDistanceLine *l = *it; if (l->isVisible() && l->isTracked()) { l->displayMovingEdges(I); } } - for (std::list::const_iterator it = - cylinders[lvl].begin(); - it != cylinders[lvl].end(); ++it) { + for (std::list::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end(); + ++it) { vpMbtDistanceCylinder *cy = *it; if (cy->isVisible() && cy->isTracked()) { cy->displayMovingEdges(I); } } - for (std::list::const_iterator it = - circles[lvl].begin(); - it != circles[lvl].end(); ++it) { + for (std::list::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) { vpMbtDistanceCircle *ci = *it; if (ci->isVisible() && ci->isTracked()) { ci->displayMovingEdges(I); @@ -1526,19 +1426,17 @@ void vpMbEdgeTracker::displayFeaturesOnImage(const vpImage &I, \param I : The image. \param _cMo : The pose of the camera used to initialize the moving edges. */ -void vpMbEdgeTracker::initMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &_cMo) +void vpMbEdgeTracker::initMovingEdge(const vpImage &I, const vpHomogeneousMatrix &_cMo) { vpMbtDistanceLine *l; - for (std::list::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { l = *it; bool isvisible = false; - for (std::list::const_iterator itindex = l->Lindex_polygon.begin(); - itindex != l->Lindex_polygon.end(); ++itindex) { + for (std::list::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end(); + ++itindex) { int index = *itindex; if (index == -1) isvisible = true; @@ -1572,8 +1470,7 @@ void vpMbEdgeTracker::initMovingEdge(const vpImage &I, } vpMbtDistanceCylinder *cy; - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { cy = *it; @@ -1583,10 +1480,8 @@ void vpMbEdgeTracker::initMovingEdge(const vpImage &I, 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)) + 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); @@ -1612,8 +1507,7 @@ void vpMbEdgeTracker::initMovingEdge(const vpImage &I, } vpMbtDistanceCircle *ci; - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { ci = *it; bool isvisible = false; @@ -1649,9 +1543,8 @@ void vpMbEdgeTracker::initMovingEdge(const vpImage &I, */ void vpMbEdgeTracker::trackMovingEdge(const vpImage &I) { - for (std::list::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { vpMbtDistanceLine *l = *it; if (l->isVisible() && l->isTracked()) { if (l->meline.size() == 0) { @@ -1661,8 +1554,7 @@ void vpMbEdgeTracker::trackMovingEdge(const vpImage &I) } } - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { vpMbtDistanceCylinder *cy = *it; if (cy->isVisible() && cy->isTracked()) { @@ -1673,8 +1565,7 @@ void vpMbEdgeTracker::trackMovingEdge(const vpImage &I) } } - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { vpMbtDistanceCircle *ci = *it; if (ci->isVisible() && ci->isTracked()) { @@ -1694,9 +1585,8 @@ void vpMbEdgeTracker::trackMovingEdge(const vpImage &I) void vpMbEdgeTracker::updateMovingEdge(const vpImage &I) { vpMbtDistanceLine *l; - for (std::list::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { if ((*it)->isTracked()) { l = *it; l->updateMovingEdge(I, cMo); @@ -1707,8 +1597,7 @@ void vpMbEdgeTracker::updateMovingEdge(const vpImage &I) } vpMbtDistanceCylinder *cy; - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { if ((*it)->isTracked()) { cy = *it; @@ -1720,8 +1609,7 @@ void vpMbEdgeTracker::updateMovingEdge(const vpImage &I) } vpMbtDistanceCircle *ci; - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { if ((*it)->isTracked()) { ci = *it; @@ -1738,9 +1626,8 @@ void vpMbEdgeTracker::updateMovingEdgeWeights() unsigned int n = 0; vpMbtDistanceLine *l; - for (std::list::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { if ((*it)->isTracked()) { l = *it; unsigned int indexLine = 0; @@ -1780,8 +1667,7 @@ void vpMbEdgeTracker::updateMovingEdgeWeights() // Same thing with cylinders as with lines vpMbtDistanceCylinder *cy; - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { if ((*it)->isTracked()) { cy = *it; @@ -1847,8 +1733,7 @@ void vpMbEdgeTracker::updateMovingEdgeWeights() // Same thing with circles as with lines vpMbtDistanceCircle *ci; - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { if ((*it)->isTracked()) { ci = *it; @@ -1897,13 +1782,11 @@ void vpMbEdgeTracker::updateMovingEdgeWeights() \param I : the image. \param _cMo : the pose of the used to re-initialize the moving edges */ -void vpMbEdgeTracker::reinitMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &_cMo) +void vpMbEdgeTracker::reinitMovingEdge(const vpImage &I, const vpHomogeneousMatrix &_cMo) { vpMbtDistanceLine *l; - for (std::list::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { if ((*it)->isTracked()) { l = *it; if (l->Reinit && l->isVisible()) @@ -1912,8 +1795,7 @@ void vpMbEdgeTracker::reinitMovingEdge(const vpImage &I, } vpMbtDistanceCylinder *cy; - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { if ((*it)->isTracked()) { cy = *it; @@ -1923,8 +1805,7 @@ void vpMbEdgeTracker::reinitMovingEdge(const vpImage &I, } vpMbtDistanceCircle *ci; - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { if ((*it)->isTracked()) { ci = *it; @@ -1938,9 +1819,7 @@ void vpMbEdgeTracker::resetMovingEdge() { for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::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]; @@ -1952,9 +1831,8 @@ void vpMbEdgeTracker::resetMovingEdge() } } - for (std::list::const_iterator it = - cylinders[i].begin(); - it != cylinders[i].end(); ++it) { + for (std::list::const_iterator it = cylinders[i].begin(); it != cylinders[i].end(); + ++it) { if ((*it)->meline1 != NULL) { delete (*it)->meline1; (*it)->meline1 = NULL; @@ -1969,9 +1847,7 @@ void vpMbEdgeTracker::resetMovingEdge() (*it)->nbFeaturel2 = 0; } - for (std::list::const_iterator it = - circles[i].begin(); - it != circles[i].end(); ++it) { + for (std::list::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) { if ((*it)->meEllipse != NULL) { delete (*it)->meEllipse; (*it)->meEllipse = NULL; @@ -1997,8 +1873,7 @@ bool vpMbEdgeTracker::samePoint(const vpPoint &P1, const vpPoint &P2) const double dy = fabs(P1.get_oY() - P2.get_oY()); double dz = fabs(P1.get_oZ() - P2.get_oZ()); - if (dx <= std::numeric_limits::epsilon() && - dy <= std::numeric_limits::epsilon() && + if (dx <= std::numeric_limits::epsilon() && dy <= std::numeric_limits::epsilon() && dz <= std::numeric_limits::epsilon()) return true; else @@ -2017,8 +1892,7 @@ bool vpMbEdgeTracker::samePoint(const vpPoint &P1, const vpPoint &P2) const \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) +void vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name) { // suppress line already in the model bool already_here = false; @@ -2027,9 +1901,7 @@ void vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { downScale(i); - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::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))) { @@ -2055,12 +1927,10 @@ void vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, if (clippingFlag != vpPolygon3D::NO_CLIPPING) l->getPolygon().setClipping(clippingFlag); - if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == - vpPolygon3D::NEAR_CLIPPING) + if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING) l->getPolygon().setNearClippingDistance(distNearClip); - if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == - vpPolygon3D::FAR_CLIPPING) + if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING) l->getPolygon().setFarClippingDistance(distFarClip); nline += 1; @@ -2082,8 +1952,7 @@ void vpMbEdgeTracker::removeLine(const std::string &name) for (unsigned int i = 0; i < scales.size(); i++) { if (scales[i]) { - for (std::list::iterator it = lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::iterator it = lines[i].begin(); it != lines[i].end(); ++it) { l = *it; if (name.compare(l->getName()) == 0) { lines[i].erase(it); @@ -2104,8 +1973,7 @@ void vpMbEdgeTracker::removeLine(const std::string &name) 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, +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; @@ -2114,17 +1982,12 @@ void vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { downScale(i); - for (std::list::const_iterator it = - circles[i].begin(); - it != circles[i].end(); ++it) { + for (std::list::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::epsilon() * - vpMath::maximum(ci->radius, r)); + 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::epsilon() * vpMath::maximum(ci->radius, r)); } } @@ -2167,8 +2030,7 @@ void vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, \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, +void vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace, const std::string &name) { bool already_here = false; @@ -2177,15 +2039,13 @@ void vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { downScale(i); - for (std::list::const_iterator it = - cylinders[i].begin(); - it != cylinders[i].end(); ++it) { + for (std::list::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::epsilon() * - vpMath::maximum(cy->radius, r)); + already_here = + (std::fabs(cy->radius - r) < std::numeric_limits::epsilon() * vpMath::maximum(cy->radius, r)); } } @@ -2218,9 +2078,7 @@ void vpMbEdgeTracker::removeCylinder(const std::string &name) for (unsigned int i = 0; i < scales.size(); i++) { if (scales[i]) { - for (std::list::iterator it = - cylinders[i].begin(); - it != cylinders[i].end(); ++it) { + for (std::list::iterator it = cylinders[i].begin(); it != cylinders[i].end(); ++it) { cy = *it; if (name.compare(cy->getName()) == 0) { cylinders[i].erase(it); @@ -2242,8 +2100,7 @@ void vpMbEdgeTracker::removeCircle(const std::string &name) for (unsigned int i = 0; i < scales.size(); i++) { if (scales[i]) { - for (std::list::iterator it = circles[i].begin(); - it != circles[i].end(); ++it) { + for (std::list::iterator it = circles[i].begin(); it != circles[i].end(); ++it) { ci = *it; if (name.compare(ci->getName()) == 0) { circles[i].erase(it); @@ -2280,8 +2137,7 @@ void vpMbEdgeTracker::addPolygon(vpMbtPolygon &p) image. \param newvisibleline : This parameter is set to true if a new face appeared. */ -void vpMbEdgeTracker::visibleFace(const vpImage &_I, - const vpHomogeneousMatrix &_cMo, +void vpMbEdgeTracker::visibleFace(const vpImage &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline) { unsigned int n; @@ -2290,15 +2146,12 @@ void vpMbEdgeTracker::visibleFace(const vpImage &_I, if (!useOgre) { // n = faces.setVisible(_I, cam, _cMo, vpMath::rad(89), vpMath::rad(89), // changed); - n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, - changed); + n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed); } else { #ifdef VISP_HAVE_OGRE - n = faces.setVisibleOgre(_I, cam, _cMo, angleAppears, angleDisappears, - changed); + n = faces.setVisibleOgre(_I, cam, _cMo, angleAppears, angleDisappears, changed); #else - n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, - changed); + n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed); #endif } @@ -2331,10 +2184,8 @@ void vpMbEdgeTracker::initFaceFromCorners(vpMbtPolygon &polygon) unsigned int nbpt = polygon.getNbPoint(); if (nbpt > 0) { for (unsigned int i = 0; i < nbpt - 1; i++) - vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], - polygon.getIndex(), polygon.getName()); - vpMbEdgeTracker::addLine(polygon.p[nbpt - 1], polygon.p[0], - polygon.getIndex(), polygon.getName()); + vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName()); + vpMbEdgeTracker::addLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName()); } } /*! @@ -2357,24 +2208,20 @@ void vpMbEdgeTracker::initFaceFromLines(vpMbtPolygon &polygon) unsigned int nbpt = polygon.getNbPoint(); if (nbpt > 0) { for (unsigned int i = 0; i < nbpt - 1; i++) - vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], - polygon.getIndex(), polygon.getName()); + vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName()); } } -unsigned int -vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines, - unsigned int &nberrors_cylinders, - unsigned int &nberrors_circles) +unsigned int vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, + unsigned int &nberrors_circles) { unsigned int nbrow = 0; nberrors_lines = 0; nberrors_cylinders = 0; nberrors_circles = 0; - for (std::list::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { vpMbtDistanceLine *l = *it; @@ -2385,8 +2232,7 @@ vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines, } } - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { vpMbtDistanceCylinder *cy = *it; @@ -2397,8 +2243,7 @@ vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines, } } - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { vpMbtDistanceCircle *ci = *it; @@ -2423,8 +2268,7 @@ vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines, 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, +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); @@ -2440,8 +2284,7 @@ void vpMbEdgeTracker::initCircle(const vpPoint &p1, const vpPoint &p2, \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, +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); @@ -2461,27 +2304,22 @@ void vpMbEdgeTracker::resetTracker() for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) { l = *it; if (l != NULL) delete l; l = NULL; } - for (std::list::const_iterator it = - cylinders[i].begin(); - it != cylinders[i].end(); ++it) { + for (std::list::const_iterator it = cylinders[i].begin(); it != cylinders[i].end(); + ++it) { cy = *it; if (cy != NULL) delete cy; cy = NULL; } - for (std::list::const_iterator it = - circles[i].begin(); - it != circles[i].end(); ++it) { + for (std::list::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) { ci = *it; if (ci != NULL) delete ci; @@ -2527,10 +2365,8 @@ void vpMbEdgeTracker::resetTracker() 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 &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbEdgeTracker::reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose) { reInitModel(I, cad_name.c_str(), cMo_, verbose); } @@ -2544,10 +2380,8 @@ void vpMbEdgeTracker::reInitModel(const vpImage &I, 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 &I, - const char *cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbEdgeTracker::reInitModel(const vpImage &I, const char *cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose) { this->cMo.eye(); vpMbtDistanceLine *l; @@ -2556,27 +2390,22 @@ void vpMbEdgeTracker::reInitModel(const vpImage &I, for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) { l = *it; if (l != NULL) delete l; l = NULL; } - for (std::list::const_iterator it = - cylinders[i].begin(); - it != cylinders[i].end(); ++it) { + for (std::list::const_iterator it = cylinders[i].begin(); it != cylinders[i].end(); + ++it) { cy = *it; if (cy != NULL) delete cy; cy = NULL; } - for (std::list::const_iterator it = - circles[i].begin(); - it != circles[i].end(); ++it) { + for (std::list::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) { ci = *it; if (ci != NULL) delete ci; @@ -2615,23 +2444,18 @@ void vpMbEdgeTracker::reInitModel(const vpImage &I, 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); + 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::const_iterator it = - lines[level].begin(); - it != lines[level].end(); ++it) { + for (std::list::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::const_iterator itme = - l->meline[a]->getMeList().begin(); + for (std::list::const_iterator itme = l->meline[a]->getMeList().begin(); itme != l->meline[a]->getMeList().end(); ++itme) { if (itme->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++; @@ -2641,20 +2465,16 @@ unsigned int vpMbEdgeTracker::getNbPoints(const unsigned int level) const } vpMbtDistanceCylinder *cy; - for (std::list::const_iterator it = - cylinders[level].begin(); - it != cylinders[level].end(); ++it) { + for (std::list::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::const_iterator itme1 = - cy->meline1->getMeList().begin(); + if (cy->isVisible() && cy->isTracked() && (cy->meline1 != NULL || cy->meline2 != NULL)) { + for (std::list::const_iterator itme1 = cy->meline1->getMeList().begin(); itme1 != cy->meline1->getMeList().end(); ++itme1) { if (itme1->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++; } - for (std::list::const_iterator itme2 = - cy->meline2->getMeList().begin(); + for (std::list::const_iterator itme2 = cy->meline2->getMeList().begin(); itme2 != cy->meline2->getMeList().end(); ++itme2) { if (itme2->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++; @@ -2663,13 +2483,10 @@ unsigned int vpMbEdgeTracker::getNbPoints(const unsigned int level) const } vpMbtDistanceCircle *ci; - for (std::list::const_iterator it = - circles[level].begin(); - it != circles[level].end(); ++it) { + for (std::list::const_iterator it = circles[level].begin(); it != circles[level].end(); ++it) { ci = *it; if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) { - for (std::list::const_iterator itme = - ci->meEllipse->getMeList().begin(); + for (std::list::const_iterator itme = ci->meEllipse->getMeList().begin(); itme != ci->meEllipse->getMeList().end(); ++itme) { if (itme->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++; @@ -2746,9 +2563,7 @@ void vpMbEdgeTracker::setScales(const std::vector &scale) */ void vpMbEdgeTracker::setFarClippingDistance(const double &dist) { - if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == - vpPolygon3D::NEAR_CLIPPING && - dist <= distNearClip) + if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip) std::cerr << "Far clipping value cannot be inferior than near clipping " "value. Far clipping won't be considered." << std::endl; @@ -2762,9 +2577,7 @@ void vpMbEdgeTracker::setFarClippingDistance(const double &dist) for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) { l = *it; l->getPolygon().setFarClippingDistance(distFarClip); } @@ -2780,9 +2593,7 @@ void vpMbEdgeTracker::setFarClippingDistance(const double &dist) */ void vpMbEdgeTracker::setNearClippingDistance(const double &dist) { - if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == - vpPolygon3D::FAR_CLIPPING && - dist >= distFarClip) + if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip) std::cerr << "Near clipping value cannot be superior than far clipping " "value. Near clipping won't be considered." << std::endl; @@ -2796,9 +2607,7 @@ void vpMbEdgeTracker::setNearClippingDistance(const double &dist) for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) { l = *it; l->getPolygon().setNearClippingDistance(distNearClip); } @@ -2822,9 +2631,7 @@ void vpMbEdgeTracker::setClipping(const unsigned int &flags) for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) { l = *it; l->getPolygon().setClipping(clippingFlag); } @@ -2847,9 +2654,8 @@ void vpMbEdgeTracker::setClipping(const unsigned int &flags) \param _I : The input image. \param _pyramid : The pyramid of image to build from the input image. */ -void vpMbEdgeTracker::initPyramid( - const vpImage &_I, - std::vector *> &_pyramid) +void vpMbEdgeTracker::initPyramid(const vpImage &_I, + std::vector *> &_pyramid) { _pyramid.resize(scales.size()); @@ -2862,25 +2668,20 @@ void vpMbEdgeTracker::initPyramid( for (unsigned int i = 1; i < _pyramid.size(); i += 1) { if (scales[i]) { unsigned int cScale = static_cast(pow(2., (int)i)); - vpImage *I = new vpImage( - _I.getHeight() / cScale, _I.getWidth() / cScale); + vpImage *I = new vpImage(_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); + 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); + 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) { + 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]; } } @@ -2898,8 +2699,7 @@ void vpMbEdgeTracker::initPyramid( \param _pyramid : The pyramid of image to clean. */ -void vpMbEdgeTracker::cleanPyramid( - std::vector *> &_pyramid) +void vpMbEdgeTracker::cleanPyramid(std::vector *> &_pyramid) { if (_pyramid.size() > 0) { _pyramid[0] = NULL; @@ -2923,14 +2723,12 @@ void vpMbEdgeTracker::cleanPyramid( \param level : Level corresponding to the list to return. \param linesList : The list of the lines of the model. */ -void vpMbEdgeTracker::getLline(std::list &linesList, - const unsigned int level) const +void vpMbEdgeTracker::getLline(std::list &linesList, const unsigned int level) const { 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."; + std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines."; throw vpException(vpException::dimensionError, errorMsg); } @@ -2947,15 +2745,12 @@ void vpMbEdgeTracker::getLline(std::list &linesList, \param level : Level corresponding to the list to return. \param cylindersList : The list of the cylinders of the model. */ -void vpMbEdgeTracker::getLcylinder( - std::list &cylindersList, - const unsigned int level) const +void vpMbEdgeTracker::getLcylinder(std::list &cylindersList, const unsigned int level) const { 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."; + std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines."; throw vpException(vpException::dimensionError, errorMsg); } @@ -2972,15 +2767,12 @@ void vpMbEdgeTracker::getLcylinder( \param level : Level corresponding to the list to return. \param circlesList : The list of the circles of the model. */ -void vpMbEdgeTracker::getLcircle( - std::list &circlesList, - const unsigned int level) const +void vpMbEdgeTracker::getLcircle(std::list &circlesList, const unsigned int level) const { 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."; + std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines."; throw vpException(vpException::dimensionError, errorMsg); } @@ -3042,9 +2834,8 @@ void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl) scaleLevel = _lvl; vpMbtDistanceLine *l; - for (std::list::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { if ((*it)->isTracked()) { l = *it; l->reinitMovingEdge(*Ipyramid[_lvl], cMo); @@ -3052,8 +2843,7 @@ void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl) } vpMbtDistanceCylinder *cy; - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { if ((*it)->isTracked()) { cy = *it; @@ -3062,8 +2852,7 @@ void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl) } vpMbtDistanceCircle *ci; - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { if ((*it)->isTracked()) { ci = *it; @@ -3083,14 +2872,11 @@ void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl) \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) +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::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) { /*(*it)->setTracked(useEdgeTracking); for(std::list::const_iterator itpoly=(*it)->Lindex_polygon.begin(); @@ -3104,17 +2890,14 @@ void vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, (*it)->setTracked(name, useEdgeTracking); } - for (std::list::const_iterator it = - cylinders[i].begin(); - it != cylinders[i].end(); ++it) { + for (std::list::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::const_iterator it = - circles[i].begin(); - it != circles[i].end(); ++it) { + for (std::list::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 index 41b5d909ad..9cd0295989 100644 --- a/modules/tracker/mbt/src/edge/vpMbtDistanceCircle.cpp +++ b/modules/tracker/mbt/src/edge/vpMbtDistanceCircle.cpp @@ -59,10 +59,9 @@ 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) + : 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) { } @@ -89,10 +88,7 @@ vpMbtDistanceCircle::~vpMbtDistanceCircle() \param cMo : The pose of the camera used to project the circle into the image. */ -void vpMbtDistanceCircle::project(const vpHomogeneousMatrix &cMo) -{ - circle->project(cMo); -} +void vpMbtDistanceCircle::project(const vpHomogeneousMatrix &cMo) { circle->project(cMo); } /*! Build a vpMbtDistanceCircle thanks to its center, 3 points (including the @@ -104,8 +100,7 @@ void vpMbtDistanceCircle::project(const vpHomogeneousMatrix &cMo) 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) +void vpMbtDistanceCircle::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r) { circle = new vpCircle; p1 = new vpPoint; @@ -123,8 +118,7 @@ void vpMbtDistanceCircle::buildFrom(const vpPoint &_p1, const vpPoint &_p2, 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); + circle->setWorldCoordinates(plane.getA(), plane.getB(), plane.getC(), _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r); } /*! @@ -149,8 +143,7 @@ void vpMbtDistanceCircle::setMovingEdge(vpMe *_me) \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 &I, - const vpHomogeneousMatrix &cMo) +bool vpMbtDistanceCircle::initMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo) { if (isvisible) { // Perspective projection @@ -168,14 +161,12 @@ bool vpMbtDistanceCircle::initMovingEdge(const vpImage &I, meEllipse->setMe(me); // meEllipse->setDisplay(vpMeSite::RANGE_RESULT) ; // TODO only for debug - meEllipse->setInitRange( - me->getRange()); // TODO: check because set to zero for lines + 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); + 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"); @@ -191,8 +182,7 @@ bool vpMbtDistanceCircle::initMovingEdge(const vpImage &I, \param I : the image. \param cMo : The pose of the camera. */ -void vpMbtDistanceCircle::trackMovingEdge(const vpImage &I, - const vpHomogeneousMatrix & /*cMo*/) +void vpMbtDistanceCircle::trackMovingEdge(const vpImage &I, const vpHomogeneousMatrix & /*cMo*/) { if (isvisible) { try { @@ -216,8 +206,7 @@ void vpMbtDistanceCircle::trackMovingEdge(const vpImage &I, \param I : the image. \param cMo : The pose of the camera. */ -void vpMbtDistanceCircle::updateMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo) +void vpMbtDistanceCircle::updateMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo) { if (isvisible) { // Perspective projection @@ -233,8 +222,7 @@ void vpMbtDistanceCircle::updateMovingEdge(const vpImage &I, vpImagePoint ic; double mu20_p, mu11_p, mu02_p; - vpMeterPixelConversion::convertEllipse(cam, *circle, ic, 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; @@ -252,8 +240,7 @@ void vpMbtDistanceCircle::updateMovingEdge(const vpImage &I, \param I : the image. \param cMo : The pose of the camera. */ -void vpMbtDistanceCircle::reinitMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo) +void vpMbtDistanceCircle::reinitMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo) { if (meEllipse != NULL) delete meEllipse; @@ -277,11 +264,8 @@ void vpMbtDistanceCircle::reinitMovingEdge(const vpImage &I, \param displayFullModel : When true, display the circle even if non visible. If false, display the circle only if visible. */ -void vpMbtDistanceCircle::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, +void vpMbtDistanceCircle::display(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { if ((isvisible && isTrackedCircle) || displayFullModel) { @@ -296,10 +280,8 @@ void vpMbtDistanceCircle::display(const vpImage &I, 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); + vpMeterPixelConversion::convertEllipse(camera, *circle, center, mu20_p, mu11_p, mu02_p); + vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, col, thickness); } } @@ -314,11 +296,8 @@ void vpMbtDistanceCircle::display(const vpImage &I, \param displayFullModel : When true, display the circle even if non visible. If false, display the circle only if visible. */ -void vpMbtDistanceCircle::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, +void vpMbtDistanceCircle::display(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { if ((isvisible && isTrackedCircle) || displayFullModel) { @@ -333,10 +312,8 @@ void vpMbtDistanceCircle::display(const vpImage &I, 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); + vpMeterPixelConversion::convertEllipse(camera, *circle, center, mu20_p, mu11_p, mu02_p); + vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, col, thickness); } } @@ -380,8 +357,7 @@ void vpMbtDistanceCircle::initInteractionMatrixError() Compute the interaction matrix and the error vector corresponding to the point to ellipse algebraic distance. */ -void vpMbtDistanceCircle::computeInteractionMatrixError( - const vpHomogeneousMatrix &cMo) +void vpMbtDistanceCircle::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo) { if (isvisible) { // Perspective projection @@ -408,9 +384,8 @@ void vpMbtDistanceCircle::computeInteractionMatrixError( unsigned int j = 0; - for (std::list::const_iterator it = - meEllipse->getMeList().begin(); - it != meEllipse->getMeList().end(); ++it) { + for (std::list::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)); @@ -419,14 +394,11 @@ void vpMbtDistanceCircle::computeInteractionMatrixError( 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; + 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 index f31ae2b4e9..81e85d4b11 100644 --- a/modules/tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp +++ b/modules/tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp @@ -62,11 +62,10 @@ 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) + : 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) { } @@ -117,8 +116,7 @@ void vpMbtDistanceCylinder::project(const vpHomogeneousMatrix &cMo) \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) +void vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r) { c = new vpCylinder; p1 = new vpPoint; @@ -148,16 +146,12 @@ void vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, 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); + 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); + 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); } /*! @@ -185,8 +179,7 @@ void vpMbtDistanceCylinder::setMovingEdge(vpMe *_me) \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 &I, - const vpHomogeneousMatrix &cMo) +bool vpMbtDistanceCylinder::initMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo) { if (isvisible) { // Perspective projection @@ -226,10 +219,8 @@ bool vpMbtDistanceCylinder::initMovingEdge(const vpImage &I, 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); + 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; @@ -324,8 +315,7 @@ bool vpMbtDistanceCylinder::initMovingEdge(const vpImage &I, \param I : the image. \param cMo : The pose of the camera. */ -void vpMbtDistanceCylinder::trackMovingEdge( - const vpImage &I, const vpHomogeneousMatrix & /*cMo*/) +void vpMbtDistanceCylinder::trackMovingEdge(const vpImage &I, const vpHomogeneousMatrix & /*cMo*/) { if (isvisible) { try { @@ -356,8 +346,7 @@ void vpMbtDistanceCylinder::trackMovingEdge( \param I : the image. \param cMo : The pose of the camera. */ -void vpMbtDistanceCylinder::updateMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo) +void vpMbtDistanceCylinder::updateMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo) { if (isvisible) { // Perspective projection @@ -386,10 +375,8 @@ void vpMbtDistanceCylinder::updateMovingEdge(const vpImage &I, 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); + 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; @@ -493,8 +480,7 @@ void vpMbtDistanceCylinder::updateMovingEdge(const vpImage &I, \param I : the image. \param cMo : The pose of the camera. */ -void vpMbtDistanceCylinder::reinitMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo) +void vpMbtDistanceCylinder::reinitMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo) { if (meline1 != NULL) delete meline1; @@ -520,11 +506,8 @@ void vpMbtDistanceCylinder::reinitMovingEdge(const vpImage &I, \param thickness : The thickness of the lines. \param displayFullModel : When true, display the circle even if non visible. */ -void vpMbtDistanceCylinder::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, +void vpMbtDistanceCylinder::display(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { if ((isvisible && isTrackedCylinder) || displayFullModel) { @@ -553,10 +536,8 @@ void vpMbtDistanceCylinder::display(const vpImage &I, 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); + 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; @@ -590,11 +571,8 @@ void vpMbtDistanceCylinder::display(const vpImage &I, \param thickness : The thickness of the lines. \param displayFullModel : When true, display the circle even if non visible. */ -void vpMbtDistanceCylinder::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, +void vpMbtDistanceCylinder::display(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { if ((isvisible && isTrackedCylinder) || displayFullModel) { @@ -623,10 +601,8 @@ void vpMbtDistanceCylinder::display(const vpImage &I, 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); + 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; @@ -664,8 +640,7 @@ void vpMbtDistanceCylinder::display(const vpImage &I, \param I : The image. */ -void vpMbtDistanceCylinder::displayMovingEdges( - const vpImage &I) +void vpMbtDistanceCylinder::displayMovingEdges(const vpImage &I) { if (meline1 != NULL) { meline1->display(I); @@ -697,8 +672,8 @@ void vpMbtDistanceCylinder::initInteractionMatrixError() Compute the interaction matrix and the error vector corresponding to the cylinder. */ -void vpMbtDistanceCylinder::computeInteractionMatrixError( - const vpHomogeneousMatrix &cMo, const vpImage &I) +void vpMbtDistanceCylinder::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, + const vpImage &I) { if (isvisible) { // Perspective projection @@ -748,9 +723,8 @@ void vpMbtDistanceCylinder::computeInteractionMatrixError( vpMeSite p; unsigned int j = 0; - for (std::list::const_iterator it = - meline1->getMeList().begin(); - it != meline1->getMeList().end(); ++it) { + for (std::list::const_iterator it = meline1->getMeList().begin(); it != meline1->getMeList().end(); + ++it) { double x = (double)it->j; double y = (double)it->i; @@ -768,16 +742,13 @@ void vpMbtDistanceCylinder::computeInteractionMatrixError( error[j] = rho1 - (x * co1 + y * si1); if (disp) - vpDisplay::displayCross(I, it->i, it->j, - (unsigned int)(error[j] * 100), - vpColor::orange, 1); + vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j] * 100), vpColor::orange, 1); j++; } - for (std::list::const_iterator it = - meline2->getMeList().begin(); - it != meline2->getMeList().end(); ++it) { + for (std::list::const_iterator it = meline2->getMeList().begin(); it != meline2->getMeList().end(); + ++it) { double x = (double)it->j; double y = (double)it->i; @@ -795,8 +766,7 @@ void vpMbtDistanceCylinder::computeInteractionMatrixError( error[j] = rho2 - (x * co2 + y * si2); if (disp) - vpDisplay::displayCross( - I, it->i, it->j, (unsigned int)(error[j] * 100), vpColor::red, 1); + 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 index 0f8d978bf8..a92db66638 100644 --- a/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp +++ b/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp @@ -57,11 +57,9 @@ 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) + : 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) { } @@ -154,8 +152,7 @@ void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L) buildPlane(P1, P2, P3, plane1); buildPlane(P1, P2, P4, plane2); - L.setWorldCoordinates(plane1.getA(), plane1.getB(), plane1.getC(), - plane1.getD(), plane2.getA(), plane2.getB(), + L.setWorldCoordinates(plane1.getA(), plane1.getB(), plane1.getC(), plane1.getD(), plane2.getA(), plane2.getB(), plane2.getC(), plane2.getD()); } @@ -190,8 +187,7 @@ void vpMbtDistanceLine::buildFrom(vpPoint &_p1, vpPoint &_p2) V2[2] = p2->get_oZ(); // if((V1-V2).sumSquare()!=0) - if (std::fabs((V1 - V2).sumSquare()) > - std::numeric_limits::epsilon()) { + if (std::fabs((V1 - V2).sumSquare()) > std::numeric_limits::epsilon()) { vpColVector V3(3); V3[0] = double(rand() % 1000) / 100; V3[1] = double(rand() % 1000) / 100; @@ -230,12 +226,10 @@ void vpMbtDistanceLine::addPolygon(const int &idx) \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) +void vpMbtDistanceLine::setTracked(const std::string &polyname, const bool &track) { unsigned int ind = 0; - for (std::list::const_iterator itpoly = Lindex_polygon.begin(); - itpoly != Lindex_polygon.end(); ++itpoly) { + for (std::list::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) { if ((*hiddenface)[(unsigned)(*itpoly)]->getName() == polyname) { Lindex_polygon_tracked[ind] = track; } @@ -271,10 +265,8 @@ void vpMbtDistanceLine::updateTracked() unsigned int ind = 0; isTrackedLineWithVisibility = false; - for (std::list::const_iterator itpoly = Lindex_polygon.begin(); - itpoly != Lindex_polygon.end(); ++itpoly) { - if ((*hiddenface)[(unsigned)(*itpoly)]->isVisible() && - Lindex_polygon_tracked[ind]) { + for (std::list::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) { + if ((*hiddenface)[(unsigned)(*itpoly)]->isVisible() && Lindex_polygon_tracked[ind]) { isTrackedLineWithVisibility = true; break; } @@ -310,8 +302,7 @@ void vpMbtDistanceLine::setMovingEdge(vpMe *_me) \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 &I, - const vpHomogeneousMatrix &cMo) +bool vpMbtDistanceLine::initMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo) { for (unsigned int i = 0; i < meline.size(); i++) { if (meline[i] != NULL) @@ -336,11 +327,9 @@ bool vpMbtDistanceLine::initMovingEdge(const vpImage &I, std::vector > linesLst; if (useScanLine) { - hiddenface->computeScanLineQuery(poly.polyClipped[0].first, - poly.polyClipped[1].first, linesLst); + 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)); + linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first)); } if (linesLst.size() == 0) { @@ -366,8 +355,7 @@ bool vpMbtDistanceLine::initMovingEdge(const vpImage &I, line->projection(); double rho, theta; // rho theta uv - vpMeterPixelConversion::convertLine(cam, line->getRho(), - line->getTheta(), rho, theta); + vpMeterPixelConversion::convertLine(cam, line->getRho(), line->getTheta(), rho, theta); while (theta > M_PI) { theta -= M_PI; @@ -387,10 +375,8 @@ bool vpMbtDistanceLine::initMovingEdge(const vpImage &I, 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); + 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); @@ -443,8 +429,7 @@ bool vpMbtDistanceLine::initMovingEdge(const vpImage &I, \param I : the image. \param cMo : The pose of the camera. */ -void vpMbtDistanceLine::trackMovingEdge(const vpImage &I, - const vpHomogeneousMatrix & /*cMo*/) +void vpMbtDistanceLine::trackMovingEdge(const vpImage &I, const vpHomogeneousMatrix & /*cMo*/) { if (isvisible) { @@ -496,8 +481,7 @@ void vpMbtDistanceLine::trackMovingEdge(const vpImage &I, \param I : the image. \param cMo : The pose of the camera. */ -void vpMbtDistanceLine::updateMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo) +void vpMbtDistanceLine::updateMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo) { if (isvisible) { p1->changeFrame(cMo); @@ -513,11 +497,9 @@ void vpMbtDistanceLine::updateMovingEdge(const vpImage &I, std::vector > linesLst; if (useScanLine) { - hiddenface->computeScanLineQuery(poly.polyClipped[0].first, - poly.polyClipped[1].first, linesLst); + 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)); + linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first)); } if (linesLst.size() != meline.size() || linesLst.size() == 0) { @@ -551,8 +533,7 @@ void vpMbtDistanceLine::updateMovingEdge(const vpImage &I, line->projection(); double rho, theta; // rho theta uv - vpMeterPixelConversion::convertLine(cam, line->getRho(), - line->getTheta(), rho, theta); + vpMeterPixelConversion::convertLine(cam, line->getRho(), line->getTheta(), rho, theta); while (theta > M_PI) { theta -= M_PI; @@ -573,12 +554,8 @@ void vpMbtDistanceLine::updateMovingEdge(const vpImage &I, 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); + 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()) { @@ -635,8 +612,7 @@ void vpMbtDistanceLine::updateMovingEdge(const vpImage &I, \param I : the image. \param cMo : The pose of the camera. */ -void vpMbtDistanceLine::reinitMovingEdge(const vpImage &I, - const vpHomogeneousMatrix &cMo) +void vpMbtDistanceLine::reinitMovingEdge(const vpImage &I, const vpHomogeneousMatrix &cMo) { for (unsigned int i = 0; i < meline.size(); i++) { if (meline[i] != NULL) @@ -664,11 +640,8 @@ void vpMbtDistanceLine::reinitMovingEdge(const vpImage &I, \param displayFullModel : If true, the line is displayed even if it is not visible. */ -void vpMbtDistanceLine::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, +void vpMbtDistanceLine::display(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { if ((isvisible && isTrackedLine) || displayFullModel) { @@ -683,39 +656,26 @@ void vpMbtDistanceLine::display(const vpImage &I, 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)) { + ((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 > linesLst; if (useScanLine && !displayFullModel) { - hiddenface->computeScanLineQuery(poly.polyClipped[0].first, - poly.polyClipped[1].first, linesLst, - true); + 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)); + 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); + 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); } @@ -734,11 +694,8 @@ void vpMbtDistanceLine::display(const vpImage &I, \param displayFullModel : If true, the line is displayed even if it is not visible. */ -void vpMbtDistanceLine::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, +void vpMbtDistanceLine::display(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { if ((isvisible && isTrackedLine) || displayFullModel) { @@ -753,39 +710,26 @@ void vpMbtDistanceLine::display(const vpImage &I, 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)) { + ((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 > linesLst; if (useScanLine && !displayFullModel) { - hiddenface->computeScanLineQuery(poly.polyClipped[0].first, - poly.polyClipped[1].first, linesLst, - true); + 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)); + 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); + 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); } @@ -838,8 +782,7 @@ void vpMbtDistanceLine::initInteractionMatrixError() Compute the interaction matrix and the error vector corresponding to the line. */ -void vpMbtDistanceLine::computeInteractionMatrixError( - const vpHomogeneousMatrix &cMo) +void vpMbtDistanceLine::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo) { if (isvisible) { try { @@ -867,8 +810,7 @@ void vpMbtDistanceLine::computeInteractionMatrixError( unsigned int j = 0; for (unsigned int i = 0; i < meline.size(); i++) { - for (std::list::const_iterator it = - meline[i]->getMeList().begin(); + for (std::list::const_iterator it = meline[i]->getMeList().begin(); it != meline[i]->getMeList().end(); ++it) { x = (double)it->j; y = (double)it->i; @@ -890,13 +832,11 @@ void vpMbtDistanceLine::computeInteractionMatrixError( } } catch (const vpException &e) { std::cerr << "Catch an exception: " << e.what() << std::endl; - std::cerr << "Set the corresponding interaction matrix part to zero." - << std::endl; + std::cerr << "Set the corresponding interaction matrix part to zero." << std::endl; unsigned int j = 0; for (unsigned int i = 0; i < meline.size(); i++) { - for (std::list::const_iterator it = - meline[i]->getMeList().begin(); + for (std::list::const_iterator it = meline[i]->getMeList().begin(); it != meline[i]->getMeList().end(); ++it) { for (unsigned int k = 0; k < 6; k++) { L[j][k] = 0.0; @@ -918,8 +858,7 @@ void vpMbtDistanceLine::computeInteractionMatrixError( \param threshold : the threshold in pixel \return true if the line is near the border of the image */ -bool vpMbtDistanceLine::closeToImageBorder(const vpImage &I, - const unsigned int threshold) +bool vpMbtDistanceLine::closeToImageBorder(const vpImage &I, const unsigned int threshold) { if (threshold > I.getWidth() || threshold > I.getHeight()) { return true; @@ -927,9 +866,8 @@ bool vpMbtDistanceLine::closeToImageBorder(const vpImage &I, if (isvisible) { for (unsigned int i = 0; i < meline.size(); i++) { - for (std::list::const_iterator it = - meline[i]->getMeList().begin(); - it != meline[i]->getMeList().end(); ++it) { + for (std::list::const_iterator it = meline[i]->getMeList().begin(); it != meline[i]->getMeList().end(); + ++it) { int i_ = it->i; int j_ = it->j; @@ -937,10 +875,8 @@ bool vpMbtDistanceLine::closeToImageBorder(const vpImage &I, return true; } - if (((unsigned int)i_ > (I.getHeight() - threshold)) || - (unsigned int)i_ < threshold || - ((unsigned int)j_ > (I.getWidth() - threshold)) || - (unsigned int)j_ < threshold) { + if (((unsigned int)i_ > (I.getHeight() - threshold)) || (unsigned int)i_ < threshold || + ((unsigned int)j_ > (I.getWidth() - threshold)) || (unsigned int)j_ < threshold) { return true; } } diff --git a/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp b/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp index 4aeedb7e23..e6b5804957 100644 --- a/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp +++ b/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp @@ -54,8 +54,7 @@ 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.) + : iPc(), a(0.), b(0.), e(0.), ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.), thresholdWeight(0.), expecteddensity(0.) { } @@ -63,9 +62,8 @@ vpMbtMeEllipse::vpMbtMeEllipse() Copy constructor. */ vpMbtMeEllipse::vpMbtMeEllipse(const vpMbtMeEllipse &meellipse) - : vpMeTracker(meellipse), iPc(meellipse.iPc), a(0.), b(0.), e(0.), ce(0.), - se(0.), mu11(0.), mu20(0.), mu02(0.), thresholdWeight(0.), - expecteddensity(0.) + : vpMeTracker(meellipse), iPc(meellipse.iPc), a(0.), b(0.), e(0.), ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.), + thresholdWeight(0.), expecteddensity(0.) { a = meellipse.a; b = meellipse.b; @@ -96,8 +94,7 @@ vpMbtMeEllipse::~vpMbtMeEllipse() { list.clear(); } \param _sumErrorRad : sum of the error per feature. \param _nbFeatures : Number of features used to compute _sumErrorRad. */ -void vpMbtMeEllipse::computeProjectionError(const vpImage &_I, - double &_sumErrorRad, +void vpMbtMeEllipse::computeProjectionError(const vpImage &_I, double &_sumErrorRad, unsigned int &_nbFeatures) { _sumErrorRad = 0; @@ -196,18 +193,15 @@ void vpMbtMeEllipse::computeProjectionError(const vpImage &_I, int height = (int)_I.getHeight(); int width = (int)_I.getWidth(); - for (std::list::iterator it = list.begin(); it != list.end(); - ++it) { + for (std::list::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())) - + 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; @@ -239,8 +233,7 @@ void vpMbtMeEllipse::computeProjectionError(const vpImage &_I, if (jImg > _I.getWidth() - 1) jImg = _I.getWidth() - 1; - gradientX += - filterX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg); + gradientX += filterX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg); } } @@ -259,8 +252,7 @@ void vpMbtMeEllipse::computeProjectionError(const vpImage &_I, if (jImg > _I.getWidth() - 1) jImg = _I.getWidth() - 1; - gradientY += - filterY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg); + gradientY += filterY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg); } } @@ -312,16 +304,14 @@ void vpMbtMeEllipse::sample(const vpImage &I) { if (!me) { vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized"); - throw(vpTrackingException(vpTrackingException::initializationError, - "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::epsilon()) { + if (std::fabs(me->getSampleStep()) <= std::numeric_limits::epsilon()) { std::cout << "In vpMbtMeEllipse::sample: "; std::cout << "function called with sample step = 0"; // return fatalError; @@ -331,9 +321,7 @@ void vpMbtMeEllipse::sample(const vpImage &I) // [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)))); + 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; @@ -356,13 +344,10 @@ void vpMbtMeEllipse::sample(const vpImage &I) // 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)) { + 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())) - + 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; @@ -398,8 +383,7 @@ void vpMbtMeEllipse::reSample(const vpImage &I) { if (!me) { vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized"); - throw(vpTrackingException(vpTrackingException::initializationError, - "Moving edges not initialized")); + throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized")); } unsigned int n = numberOfSignal(); @@ -417,18 +401,15 @@ void vpMbtMeEllipse::reSample(const vpImage &I) void vpMbtMeEllipse::updateTheta() { vpMeSite p_me; - for (std::list::iterator it = list.begin(); it != list.end(); - ++it) { + for (std::list::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 - double 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())) - + double 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; @@ -443,8 +424,7 @@ void vpMbtMeEllipse::updateTheta() void vpMbtMeEllipse::suppressPoints() { // Loop through list of sites to track - for (std::list::iterator itList = list.begin(); - itList != list.end();) { + for (std::list::iterator itList = list.begin(); itList != list.end();) { vpMeSite s = *itList; // current reference pixel if (s.getState() != vpMeSite::NO_SUPPRESSION) itList = list.erase(itList); @@ -467,9 +447,8 @@ void vpMbtMeEllipse::display(const vpImage &I, vpColor col) vpDisplay::displayEllipse(I, iPc, mu20, mu11, mu02, true, col); } -void vpMbtMeEllipse::initTracking(const vpImage &I, - const vpImagePoint &ic, double mu20_p, - double mu11_p, double mu02_p) +void vpMbtMeEllipse::initTracking(const vpImage &I, const vpImagePoint &ic, double mu20_p, double mu11_p, + double mu02_p) { iPc = ic; mu20 = mu20_p; @@ -478,8 +457,7 @@ void vpMbtMeEllipse::initTracking(const vpImage &I, if (std::fabs(mu11_p) > std::numeric_limits::epsilon()) { - double val_p = - sqrt(vpMath::sqr(mu20_p - mu02_p) + 4 * vpMath::sqr(mu11_p)); + 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); @@ -520,8 +498,7 @@ void vpMbtMeEllipse::track(const vpImage &I) } } -void vpMbtMeEllipse::updateParameters(const vpImage &I, - const vpImagePoint &ic, double mu20_p, +void vpMbtMeEllipse::updateParameters(const vpImage &I, const vpImagePoint &ic, double mu20_p, double mu11_p, double mu02_p) { iPc = ic; @@ -531,8 +508,7 @@ void vpMbtMeEllipse::updateParameters(const vpImage &I, if (std::fabs(mu11_p) > std::numeric_limits::epsilon()) { - double val_p = - sqrt(vpMath::sqr(mu20_p - mu02_p) + 4 * vpMath::sqr(mu11_p)); + 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); diff --git a/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp b/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp index 89621a83c6..5a5d680277 100644 --- a/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp +++ b/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp @@ -67,9 +67,8 @@ static void normalizeAngle(double &delta) 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.) + : 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.) { } @@ -92,10 +91,8 @@ vpMbtMeLine::~vpMbtMeLine() { list.clear(); } \param rho_ : The \f$\rho\f$ parameter \param theta_ : The \f$\theta\f$ parameter */ -void vpMbtMeLine::initTracking(const vpImage &I, - const vpImagePoint &ip1, - const vpImagePoint &ip2, double rho_, - double theta_) +void vpMbtMeLine::initTracking(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, + double rho_, double theta_) { vpCDEBUG(1) << " begin vpMeLine::initTracking()" << std::endl; @@ -142,11 +139,9 @@ void vpMbtMeLine::sample(const vpImage &I) double n_sample; // if (me->getSampleStep==0) - if (std::fabs(me->getSampleStep()) <= - std::numeric_limits::epsilon()) { - throw(vpTrackingException(vpTrackingException::fatalError, - "Function vpMbtMeLine::sample() called with " - "moving-edges sample step = 0")); + if (std::fabs(me->getSampleStep()) <= std::numeric_limits::epsilon()) { + throw(vpTrackingException(vpTrackingException::fatalError, "Function vpMbtMeLine::sample() called with " + "moving-edges sample step = 0")); } // i, j portions of the line_p @@ -174,8 +169,7 @@ void vpMbtMeLine::sample(const vpImage &I) 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, + 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); @@ -230,8 +224,7 @@ void vpMbtMeLine::suppressPoints(const vpImage &I) } } - if (outOfImage(s.i, s.j, (int)(me->getRange() + me->getMaskSize() + 1), - (int)I.getHeight(), (int)I.getWidth())) { + if (outOfImage(s.i, s.j, (int)(me->getRange() + me->getMaskSize() + 1), (int)I.getHeight(), (int)I.getWidth())) { s.setState(vpMeSite::TOO_NEAR); } @@ -257,10 +250,8 @@ void vpMbtMeLine::seekExtremities(const vpImage &I) double n_sample; // if (me->getSampleStep()==0) - if (std::fabs(me->getSampleStep()) <= - std::numeric_limits::epsilon()) { - throw(vpTrackingException(vpTrackingException::fatalError, - "Function called with sample step = 0")); + if (std::fabs(me->getSampleStep()) <= std::numeric_limits::epsilon()) { + throw(vpTrackingException(vpTrackingException::fatalError, "Function called with sample step = 0")); } // i, j portions of the line_p @@ -294,9 +285,7 @@ void vpMbtMeLine::seekExtremities(const vpImage &I) 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)) { + } 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) { @@ -321,9 +310,7 @@ void vpMbtMeLine::seekExtremities(const vpImage &I) 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)) { + 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) { @@ -350,8 +337,7 @@ void vpMbtMeLine::seekExtremities(const vpImage &I) \param _sumErrorRad : sum of the error per feature. \param _nbFeatures : Number of features used to compute _sumErrorRad. */ -void vpMbtMeLine::computeProjectionError(const vpImage &_I, - double &_sumErrorRad, +void vpMbtMeLine::computeProjectionError(const vpImage &_I, double &_sumErrorRad, unsigned int &_nbFeatures) { _sumErrorRad = 0; @@ -459,8 +445,7 @@ void vpMbtMeLine::computeProjectionError(const vpImage &_I, double offset = std::floor(filterX.getRows() / 2.0f); - for (std::list::iterator it = list.begin(); it != list.end(); - ++it) { + for (std::list::iterator it = list.begin(); it != list.end(); ++it) { if (iter != 0 && iter + 1 != list.size()) { double gradientX = 0; double gradientY = 0; @@ -483,8 +468,7 @@ void vpMbtMeLine::computeProjectionError(const vpImage &_I, if (jImg > _I.getWidth() - 1) jImg = _I.getWidth() - 1; - gradientX += - filterX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg); + gradientX += filterX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg); } } @@ -503,8 +487,7 @@ void vpMbtMeLine::computeProjectionError(const vpImage &_I, if (jImg > _I.getWidth() - 1) jImg = _I.getWidth() - 1; - gradientY += - filterY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg); + gradientY += filterY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg); } } @@ -581,8 +564,7 @@ void vpMbtMeLine::reSample(const vpImage &I) \param ip1 : The first extremity of the line. \param ip2 : The second extremity of the line. */ -void vpMbtMeLine::reSample(const vpImage &I, - const vpImagePoint &ip1, const vpImagePoint &ip2) +void vpMbtMeLine::reSample(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2) { size_t n = list.size(); @@ -612,8 +594,7 @@ void vpMbtMeLine::updateDelta() // if(fabs(theta) == M_PI ) if (std::fabs(std::fabs(theta) - M_PI) <= - vpMath::maximum(std::fabs(theta), (double)M_PI) * - std::numeric_limits::epsilon()) { + vpMath::maximum(std::fabs(theta), (double)M_PI) * std::numeric_limits::epsilon()) { theta = 0; } @@ -626,8 +607,7 @@ void vpMbtMeLine::updateDelta() delta = -theta + M_PI / 2.0; normalizeAngle(delta); - for (std::list::iterator it = list.begin(); it != list.end(); - ++it) { + for (std::list::iterator it = list.begin(); it != list.end(); ++it) { p_me = *it; p_me.alpha = delta; p_me.mask_sign = sign; @@ -663,8 +643,7 @@ void vpMbtMeLine::track(const vpImage &I) \param theta_ : The \f$\theta\f$ parameter used in the line's polar equation. */ -void vpMbtMeLine::updateParameters(const vpImage &I, - double rho_, double theta_) +void vpMbtMeLine::updateParameters(const vpImage &I, double rho_, double theta_) { this->rho = rho_; this->theta = theta_; @@ -694,10 +673,8 @@ void vpMbtMeLine::updateParameters(const vpImage &I, \param theta_ : The \f$\theta\f$ parameter used in the line's polar equation. */ -void vpMbtMeLine::updateParameters(const vpImage &I, - const vpImagePoint &ip1, - const vpImagePoint &ip2, double rho_, - double theta_) +void vpMbtMeLine::updateParameters(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, + double rho_, double theta_) { this->rho = rho_; this->theta = theta_; @@ -728,8 +705,7 @@ void vpMbtMeLine::setExtremities() double j_max = -1; // Loop through list of sites to track - for (std::list::const_iterator it = list.begin(); - it != list.end(); ++it) { + for (std::list::const_iterator it = list.begin(); it != list.end(); ++it) { vpMeSite s = *it; // current reference pixel if (s.ifloat < i_min) { i_min = s.ifloat; @@ -750,8 +726,7 @@ void vpMbtMeLine::setExtremities() } if (fabs(i_min - i_max) < 25) { - for (std::list::const_iterator it = list.begin(); - it != list.end(); ++it) { + for (std::list::const_iterator it = list.begin(); it != list.end(); ++it) { vpMeSite s = *it; // current reference pixel if (s.jfloat < j_min) { i_min = s.ifloat; @@ -777,10 +752,7 @@ void vpMbtMeLine::setExtremities() bubbleSortI(); } -static bool sortByI(const vpMeSite &s1, const vpMeSite &s2) -{ - return (s1.ifloat > s2.ifloat); -} +static bool sortByI(const vpMeSite &s1, const vpMeSite &s2) { return (s1.ifloat > s2.ifloat); } void vpMbtMeLine::bubbleSortI() { @@ -803,10 +775,7 @@ void vpMbtMeLine::bubbleSortI() list.sort(sortByI); } -static bool sortByJ(const vpMeSite &s1, const vpMeSite &s2) -{ - return (s1.jfloat > s2.jfloat); -} +static bool sortByJ(const vpMeSite &s1, const vpMeSite &s2) { return (s1.jfloat > s2.jfloat); } void vpMbtMeLine::bubbleSortJ() { diff --git a/modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp b/modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp index 5d01438a03..b5b281c15b 100644 --- a/modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp +++ b/modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp @@ -117,11 +117,9 @@ void vpMbtXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) bool ecm_node = false; bool lod_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case camera: { @@ -153,46 +151,31 @@ void vpMbtXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) } 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; + 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; + 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; + 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; + std::cout << "lod : min line length threshold : " << minLineLengthThreshold << " (default)" << std::endl; + std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << " (default)" << std::endl; } } @@ -211,11 +194,9 @@ void vpMbtXmlParser::read_ecm(xmlDocPtr doc, xmlNodePtr node) bool contrast_node = false; bool sample_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case mask: { @@ -244,29 +225,22 @@ void vpMbtXmlParser::read_ecm(xmlDocPtr doc, xmlNodePtr node) } 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; + 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; + 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; + 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; + std::cout << "ecm : sample : sample_step : " << this->m_ecm.getSampleStep() << " (default)" << std::endl; } } @@ -285,11 +259,9 @@ void vpMbtXmlParser::read_sample(xmlDocPtr doc, xmlNodePtr node) // current data values. double d_stp = this->m_ecm.getSampleStep(); - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case step: { @@ -309,11 +281,9 @@ void vpMbtXmlParser::read_sample(xmlDocPtr doc, xmlNodePtr node) // this->m_ecm.setNbTotalSample(d_nb_sample); if (!step_node) - std::cout << "ecm : sample : sample_step : " - << this->m_ecm.getSampleStep() << " (default)" << std::endl; + 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; + std::cout << "ecm : sample : sample_step : " << this->m_ecm.getSampleStep() << std::endl; } /*! @@ -333,11 +303,9 @@ void vpMbtXmlParser::read_sample_deprecated(xmlDocPtr doc, xmlNodePtr node) 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) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case step: { @@ -361,11 +329,9 @@ void vpMbtXmlParser::read_sample_deprecated(xmlDocPtr doc, xmlNodePtr node) // this->m_ecm.setNbTotalSample(d_nb_sample); if (!step_node) - std::cout << "[DEPRECATED] sample : sample_step : " - << this->m_ecm.getSampleStep() << " (default)" << std::endl; + 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; + std::cout << "[DEPRECATED] sample : sample_step : " << this->m_ecm.getSampleStep() << std::endl; // if(!nb_sample_node) // std::cout <<"sample : n_total_sample : "<< @@ -375,8 +341,7 @@ void vpMbtXmlParser::read_sample_deprecated(xmlDocPtr doc, xmlNodePtr node) // this->m_ecm.getNbTotalSample()<m_ecm.getMaskSize(); unsigned int d_nb_mask = this->m_ecm.getMaskNumber(); - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case size: { @@ -429,18 +392,14 @@ void vpMbtXmlParser::read_mask(xmlDocPtr doc, xmlNodePtr node) this->m_ecm.setMaskNumber(d_nb_mask); if (!size_node) - std::cout << "ecm : mask : size : " << this->m_ecm.getMaskSize() - << " (default)" << std::endl; + std::cout << "ecm : mask : size : " << this->m_ecm.getMaskSize() << " (default)" << std::endl; else - std::cout << "ecm : mask : size : " << this->m_ecm.getMaskSize() - << std::endl; + 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; + 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; + std::cout << "ecm : mask : nb_mask : " << this->m_ecm.getMaskNumber() << std::endl; } /*! @@ -458,11 +417,9 @@ void vpMbtXmlParser::read_range(xmlDocPtr doc, xmlNodePtr node) // current data values. unsigned int m_range_tracking = this->m_ecm.getRange(); - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case tracking: { @@ -481,11 +438,9 @@ void vpMbtXmlParser::read_range(xmlDocPtr doc, xmlNodePtr node) this->m_ecm.setRange(m_range_tracking); if (!tracking_node) - std::cout << "ecm : range : tracking : " << this->m_ecm.getRange() - << " (default)" << std::endl; + std::cout << "ecm : range : tracking : " << this->m_ecm.getRange() << " (default)" << std::endl; else - std::cout << "ecm : range : tracking : " << this->m_ecm.getRange() - << std::endl; + std::cout << "ecm : range : tracking : " << this->m_ecm.getRange() << std::endl; } /*! @@ -507,11 +462,9 @@ void vpMbtXmlParser::read_contrast(xmlDocPtr doc, xmlNodePtr node) double d_mu1 = this->m_ecm.getMu1(); double d_mu2 = this->m_ecm.getMu2(); - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case edge_threshold: { @@ -540,21 +493,17 @@ void vpMbtXmlParser::read_contrast(xmlDocPtr doc, xmlNodePtr node) this->m_ecm.setThreshold(d_edge_threshold); if (!edge_threshold_node) - std::cout << "ecm : contrast : threshold " << this->m_ecm.getThreshold() - << " (default)" << std::endl; + std::cout << "ecm : contrast : threshold " << this->m_ecm.getThreshold() << " (default)" << std::endl; else - std::cout << "ecm : contrast : threshold " << this->m_ecm.getThreshold() - << std::endl; + 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; + 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; + std::cout << "ecm : contrast : mu2 " << this->m_ecm.getMu2() << " (default)" << std::endl; else std::cout << "ecm : contrast : mu2 " << this->m_ecm.getMu2() << std::endl; } diff --git a/modules/tracker/mbt/src/hybrid/vpMbEdgeKltMultiTracker.cpp b/modules/tracker/mbt/src/hybrid/vpMbEdgeKltMultiTracker.cpp index 63b0026f0b..3194185d35 100644 --- a/modules/tracker/mbt/src/hybrid/vpMbEdgeKltMultiTracker.cpp +++ b/modules/tracker/mbt/src/hybrid/vpMbEdgeKltMultiTracker.cpp @@ -45,8 +45,7 @@ #include -#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && \ - (VISP_HAVE_OPENCV_VERSION >= 0x020100) +#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100) #include #include @@ -56,11 +55,9 @@ Basic constructor */ vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker() - : vpMbEdgeMultiTracker(), vpMbKltMultiTracker(), m_factorKLT(0.65), - m_factorMBT(0.35), thresholdKLT(2.), thresholdMBT(2.), - m_mapOfCameraTransformationMatrix(), m_referenceCameraName("Camera"), - m_nbrow(0), m_L_hybridMulti(), m_error_hybridMulti(), m_w_hybridMulti(), - m_weightedError_hybridMulti() + : vpMbEdgeMultiTracker(), vpMbKltMultiTracker(), m_factorKLT(0.65), m_factorMBT(0.35), thresholdKLT(2.), + thresholdMBT(2.), m_mapOfCameraTransformationMatrix(), m_referenceCameraName("Camera"), m_nbrow(0), + m_L_hybridMulti(), m_error_hybridMulti(), m_w_hybridMulti(), m_weightedError_hybridMulti() { // Add default camera transformation matrix m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix(); @@ -75,17 +72,13 @@ vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker() \param nbCameras : Number of cameras to use. */ vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker(const unsigned int nbCameras) - : vpMbEdgeMultiTracker(nbCameras), vpMbKltMultiTracker(nbCameras), - m_factorKLT(0.65), m_factorMBT(0.35), thresholdKLT(2.), thresholdMBT(2.), - m_mapOfCameraTransformationMatrix(), m_referenceCameraName("Camera"), - m_nbrow(0), m_L_hybridMulti(), m_error_hybridMulti(), m_w_hybridMulti(), - m_weightedError_hybridMulti() + : vpMbEdgeMultiTracker(nbCameras), vpMbKltMultiTracker(nbCameras), m_factorKLT(0.65), m_factorMBT(0.35), + thresholdKLT(2.), thresholdMBT(2.), m_mapOfCameraTransformationMatrix(), m_referenceCameraName("Camera"), + m_nbrow(0), m_L_hybridMulti(), m_error_hybridMulti(), m_w_hybridMulti(), m_weightedError_hybridMulti() { if (nbCameras == 0) { - throw vpException( - vpTrackingException::fatalError, - "Cannot construct a vpMbkltMultiTracker with no camera !"); + throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbkltMultiTracker with no camera !"); } else if (nbCameras == 1) { // Add default camera transformation matrix m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix(); @@ -118,13 +111,10 @@ vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker(const unsigned int nbCameras) \param cameraNames : List of camera names. */ -vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker( - const std::vector &cameraNames) - : vpMbEdgeMultiTracker(cameraNames), vpMbKltMultiTracker(cameraNames), - m_factorKLT(0.65), m_factorMBT(0.35), thresholdKLT(2.), thresholdMBT(2.), - m_mapOfCameraTransformationMatrix(), - m_referenceCameraName(cameraNames.front()), m_nbrow(0), m_L_hybridMulti(), - m_error_hybridMulti(), m_w_hybridMulti(), m_weightedError_hybridMulti() +vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker(const std::vector &cameraNames) + : vpMbEdgeMultiTracker(cameraNames), vpMbKltMultiTracker(cameraNames), m_factorKLT(0.65), m_factorMBT(0.35), + thresholdKLT(2.), thresholdMBT(2.), m_mapOfCameraTransformationMatrix(), m_referenceCameraName(cameraNames.front()), + m_nbrow(0), m_L_hybridMulti(), m_error_hybridMulti(), m_w_hybridMulti(), m_weightedError_hybridMulti() { m_lambda = 0.8; m_maxIter = 200; @@ -132,16 +122,14 @@ vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker( vpMbEdgeKltMultiTracker::~vpMbEdgeKltMultiTracker() {} -void vpMbEdgeKltMultiTracker::computeVVS( - std::map *> &mapOfImages, - const unsigned int lvl) +void vpMbEdgeKltMultiTracker::computeVVS(std::map *> &mapOfImages, + const unsigned int lvl) { m_nbrow = initMbtTracking(mapOfImages, lvl); if (m_nbInfos < 4 && m_nbrow < 4) { - throw vpTrackingException(vpTrackingException::notEnoughPointError, - "Error: not enough features"); + throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features"); } else if (m_nbrow < 4) { m_nbrow = 0; } @@ -150,14 +138,12 @@ void vpMbEdgeKltMultiTracker::computeVVS( double factorKLT = m_factorKLT; if (m_nbrow < 4) { factorKLT = 1.; - std::cerr << "There are less than 4 KLT features, set factorKLT = 1. !" - << std::endl; + std::cerr << "There are less than 4 KLT features, set factorKLT = 1. !" << std::endl; } if (m_nbInfos < 4) { factorMBT = 1.; - std::cerr << "There are less than 4 moving edges, set factorMBT = 1. !" - << std::endl; + std::cerr << "There are less than 4 moving edges, set factorMBT = 1. !" << std::endl; m_nbInfos = 0; } @@ -173,8 +159,7 @@ void vpMbEdgeKltMultiTracker::computeVVS( // Create the map of VelocityTwistMatrices std::map mapOfVelocityTwist; - for (std::map::const_iterator it = - m_mapOfCameraTransformationMatrix.begin(); + for (std::map::const_iterator it = m_mapOfCameraTransformationMatrix.begin(); it != m_mapOfCameraTransformationMatrix.end(); ++it) { vpVelocityTwistMatrix cVo; cVo.buildFrom(it->second); @@ -198,8 +183,7 @@ void vpMbEdgeKltMultiTracker::computeVVS( computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist); bool reStartFromLastIncrement = false; - computeVVSCheckLevenbergMarquardt(iter, m_error_hybridMulti, m_error_prev, - cMoPrev, mu, reStartFromLastIncrement, + computeVVSCheckLevenbergMarquardt(iter, m_error_hybridMulti, m_error_prev, cMoPrev, mu, reStartFromLastIncrement, &m_w_prev); if (reStartFromLastIncrement) { ctTc0 = ctTc0_Prev; @@ -211,8 +195,7 @@ void vpMbEdgeKltMultiTracker::computeVVS( // Set weight for m_w with the good weighting between MBT and KLT for (unsigned int cpt = 0; cpt < m_nbrow + 2 * m_nbInfos; cpt++) { if (cpt < m_nbrow) { - m_w_hybridMulti[cpt] = - (m_w_hybridMulti[cpt] * m_factor[cpt]) * factorMBT; + m_w_hybridMulti[cpt] = (m_w_hybridMulti[cpt] * m_factor[cpt]) * factorMBT; } else { m_w_hybridMulti[cpt] *= factorKLT; } @@ -232,13 +215,11 @@ void vpMbEdgeKltMultiTracker::computeVVS( double num = 0; double den = 0; - for (unsigned int i = 0; i < m_weightedError_hybridMulti.getRows(); - i++) { + for (unsigned int i = 0; i < m_weightedError_hybridMulti.getRows(); i++) { num += m_w_hybridMulti[i] * vpMath::sqr(m_error_hybridMulti[i]); den += m_w_hybridMulti[i]; - m_weightedError_hybridMulti[i] = - m_error_hybridMulti[i] * m_w_hybridMulti[i]; + m_weightedError_hybridMulti[i] = m_error_hybridMulti[i] * m_w_hybridMulti[i]; if (m_computeInteraction) { for (unsigned int j = 0; j < 6; j++) { m_L_hybridMulti[i][j] *= m_w_hybridMulti[i]; @@ -248,10 +229,8 @@ void vpMbEdgeKltMultiTracker::computeVVS( residu = sqrt(num / den); - computeVVSPoseEstimation(isoJoIdentity, iter, m_L_hybridMulti, LTL, - m_weightedError_hybridMulti, - m_error_hybridMulti, m_error_prev, LTR, mu, v, - &m_w_hybridMulti, &m_w_prev); + computeVVSPoseEstimation(isoJoIdentity, iter, m_L_hybridMulti, LTL, m_weightedError_hybridMulti, + m_error_hybridMulti, m_error_prev, LTR, mu, v, &m_w_hybridMulti, &m_w_prev); cMoPrev = cMo; ctTc0_Prev = ctTc0; @@ -262,8 +241,7 @@ void vpMbEdgeKltMultiTracker::computeVVS( iter++; } - computeCovarianceMatrixVVS(isoJoIdentity, m_w_hybridMulti, cMoPrev, L_true, - LVJ_true, m_error_hybridMulti); + computeCovarianceMatrixVVS(isoJoIdentity, m_w_hybridMulti, cMoPrev, L_true, LVJ_true, m_error_hybridMulti); } void vpMbEdgeKltMultiTracker::computeVVSInit() @@ -277,8 +255,7 @@ void vpMbEdgeKltMultiTracker::computeVVSInit() m_w_hybridMulti.resize(totalNbRows, false); m_w_hybridMulti = 1; - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { vpMbKltTracker *klt = it->second; klt->computeVVSInit(); @@ -299,8 +276,7 @@ void vpMbEdgeKltMultiTracker::computeVVSInteractionMatrixAndResidu( unsigned int startIdx = 0; if (m_nbrow >= 4) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { vpMbEdgeTracker *edge = it->second; @@ -310,8 +286,7 @@ void vpMbEdgeKltMultiTracker::computeVVSInteractionMatrixAndResidu( edge->computeVVSInteractionMatrixAndResidu(*mapOfImages[it->first]); // Stack interaction matrix for MBT - m_L_hybridMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], - startIdx, 0); + m_L_hybridMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], startIdx, 0); // Stack residual for MBT m_error_hybridMulti.insert(startIdx, edge->m_error_edge); @@ -319,8 +294,7 @@ void vpMbEdgeKltMultiTracker::computeVVSInteractionMatrixAndResidu( } } - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { vpMbKltTracker *klt = it->second; @@ -333,17 +307,14 @@ void vpMbEdgeKltMultiTracker::computeVVSInteractionMatrixAndResidu( klt->ctTc0 = ctTc0; klt->computeVVSInteractionMatrixAndResidu(); } else { - vpHomogeneousMatrix c_curr_tTc_curr0 = - m_mapOfCameraTransformationMatrix[it->first] * cMo * - klt->c0Mo.inverse(); + vpHomogeneousMatrix c_curr_tTc_curr0 = m_mapOfCameraTransformationMatrix[it->first] * cMo * klt->c0Mo.inverse(); klt->ctTc0 = c_curr_tTc_curr0; klt->computeVVSInteractionMatrixAndResidu(); } // Stack residual and interaction matrix m_error_hybridMulti.insert(startIdx, klt->m_error_klt); - m_L_hybridMulti.insert(klt->m_L_klt * mapOfVelocityTwist[it->first], - startIdx, 0); + m_L_hybridMulti.insert(klt->m_L_klt * mapOfVelocityTwist[it->first], startIdx, 0); startIdx += 2 * klt->m_nbInfos; } @@ -354,8 +325,7 @@ void vpMbEdgeKltMultiTracker::computeVVSWeights() { unsigned int startIdx = 0; - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { vpMbEdgeTracker *edge = it->second; @@ -366,8 +336,7 @@ void vpMbEdgeKltMultiTracker::computeVVSWeights() startIdx += edge->m_w_edge.getRows(); } - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { vpMbKltTracker *klt = it->second; @@ -390,37 +359,28 @@ void vpMbEdgeKltMultiTracker::computeVVSWeights() \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeKltMultiTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, - const vpColor &col, - const unsigned int thickness, +void vpMbEdgeKltMultiTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { - vpMbEdgeMultiTracker::display(I, cMo_, cam_, col, thickness, - displayFullModel); + vpMbEdgeMultiTracker::display(I, cMo_, cam_, col, thickness, displayFullModel); // Display only features for KLT trackers - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { - for (std::list::const_iterator it_pts = - it_klt->second->kltPolygons.begin(); + for (std::list::const_iterator it_pts = it_klt->second->kltPolygons.begin(); it_pts != it_klt->second->kltPolygons.end(); ++it_pts) { vpMbtDistanceKltPoints *kltpoly = *it_pts; - if (displayFeatures && kltpoly->hasEnoughPoints() && - kltpoly->isTracked() && kltpoly->polygon->isVisible()) { + if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) { kltpoly->displayPrimitive(I); } } - for (std::list::const_iterator it_cyl = - it_klt->second->kltCylinders.begin(); + for (std::list::const_iterator it_cyl = it_klt->second->kltCylinders.begin(); it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl; - if (displayFeatures && kltPolyCylinder->isTracked() && - kltPolyCylinder->hasEnoughPoints()) + if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) kltPolyCylinder->displayPrimitive(I); } } @@ -437,39 +397,30 @@ void vpMbEdgeKltMultiTracker::display(const vpImage &I, \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeKltMultiTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, - const vpColor &color, - const unsigned int thickness, - const bool displayFullModel) +void vpMbEdgeKltMultiTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &cam_, const vpColor &color, + const unsigned int thickness, const bool displayFullModel) { - vpMbEdgeMultiTracker::display(I, cMo_, cam_, color, thickness, - displayFullModel); + vpMbEdgeMultiTracker::display(I, cMo_, cam_, color, thickness, displayFullModel); // vpMbKltMultiTracker::display(I, cMo_, cam_, color, thickness, // displayFullModel); // Display only features for KLT trackers - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { - for (std::list::const_iterator it_pts = - it_klt->second->kltPolygons.begin(); + for (std::list::const_iterator it_pts = it_klt->second->kltPolygons.begin(); it_pts != it_klt->second->kltPolygons.end(); ++it_pts) { vpMbtDistanceKltPoints *kltpoly = *it_pts; - if (displayFeatures && kltpoly->hasEnoughPoints() && - kltpoly->isTracked() && kltpoly->polygon->isVisible()) { + if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) { kltpoly->displayPrimitive(I); } } - for (std::list::const_iterator it_cyl = - it_klt->second->kltCylinders.begin(); + for (std::list::const_iterator it_cyl = it_klt->second->kltCylinders.begin(); it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl; - if (displayFeatures && kltPolyCylinder->isTracked() && - kltPolyCylinder->hasEnoughPoints()) + if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) kltPolyCylinder->displayPrimitive(I); } } @@ -489,29 +440,23 @@ void vpMbEdgeKltMultiTracker::display(const vpImage &I, \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeKltMultiTracker::display( - const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, const vpCameraParameters &cam2, - const vpColor &color, const unsigned int thickness, - const bool displayFullModel) +void vpMbEdgeKltMultiTracker::display(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, + const vpCameraParameters &cam1, const vpCameraParameters &cam2, + const vpColor &color, const unsigned int thickness, const bool displayFullModel) { - vpMbEdgeMultiTracker::display(I1, I2, c1Mo, c2Mo, cam1, cam2, color, - thickness, displayFullModel); + vpMbEdgeMultiTracker::display(I1, I2, c1Mo, c2Mo, cam1, cam2, color, thickness, displayFullModel); // vpMbKltMultiTracker::display(I1, I2, c1Mo, c2Mo, cam1, cam2, color, // thickness, displayFullModel); // Display only features for KLT trackers bool first = true; - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { - for (std::list::const_iterator it_pts = - it_klt->second->kltPolygons.begin(); + for (std::list::const_iterator it_pts = it_klt->second->kltPolygons.begin(); it_pts != it_klt->second->kltPolygons.end(); ++it_pts) { vpMbtDistanceKltPoints *kltpoly = *it_pts; - if (displayFeatures && kltpoly->hasEnoughPoints() && - kltpoly->isTracked() && kltpoly->polygon->isVisible()) { + if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) { if (first) { kltpoly->displayPrimitive(I1); } else { @@ -520,12 +465,10 @@ void vpMbEdgeKltMultiTracker::display( } } - for (std::list::const_iterator it_cyl = - it_klt->second->kltCylinders.begin(); + for (std::list::const_iterator it_cyl = it_klt->second->kltCylinders.begin(); it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl; - if (displayFeatures && kltPolyCylinder->isTracked() && - kltPolyCylinder->hasEnoughPoints()) { + if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) { if (first) { kltPolyCylinder->displayPrimitive(I1); } else { @@ -552,29 +495,23 @@ void vpMbEdgeKltMultiTracker::display( \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeKltMultiTracker::display( - const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, const vpCameraParameters &cam2, - const vpColor &color, const unsigned int thickness, - const bool displayFullModel) +void vpMbEdgeKltMultiTracker::display(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, + const vpCameraParameters &cam1, const vpCameraParameters &cam2, + const vpColor &color, const unsigned int thickness, const bool displayFullModel) { - vpMbEdgeMultiTracker::display(I1, I2, c1Mo, c2Mo, cam1, cam2, color, - thickness, displayFullModel); + vpMbEdgeMultiTracker::display(I1, I2, c1Mo, c2Mo, cam1, cam2, color, thickness, displayFullModel); // vpMbKltMultiTracker::display(I1, I2, c1Mo, c2Mo, cam1, cam2, color, // thickness, displayFullModel); // Display only features for KLT trackers (not the model) bool first = true; - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { - for (std::list::const_iterator it_pts = - it_klt->second->kltPolygons.begin(); + for (std::list::const_iterator it_pts = it_klt->second->kltPolygons.begin(); it_pts != it_klt->second->kltPolygons.end(); ++it_pts) { vpMbtDistanceKltPoints *kltpoly = *it_pts; - if (displayFeatures && kltpoly->hasEnoughPoints() && - kltpoly->isTracked() && kltpoly->polygon->isVisible()) { + if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) { if (first) { kltpoly->displayPrimitive(I1); } else { @@ -583,12 +520,10 @@ void vpMbEdgeKltMultiTracker::display( } } - for (std::list::const_iterator it_cyl = - it_klt->second->kltCylinders.begin(); + for (std::list::const_iterator it_cyl = it_klt->second->kltCylinders.begin(); it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl; - if (displayFeatures && kltPolyCylinder->isTracked() && - kltPolyCylinder->hasEnoughPoints()) { + if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) { if (first) { kltPolyCylinder->displayPrimitive(I1); } else { @@ -612,43 +547,33 @@ void vpMbEdgeKltMultiTracker::display( \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeKltMultiTracker::display( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness, - const bool displayFullModel) +void vpMbEdgeKltMultiTracker::display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, + const vpColor &col, const unsigned int thickness, const bool displayFullModel) { - vpMbEdgeMultiTracker::display(mapOfImages, mapOfCameraPoses, - mapOfCameraParameters, col, thickness, - displayFullModel); + vpMbEdgeMultiTracker::display(mapOfImages, mapOfCameraPoses, mapOfCameraParameters, col, thickness, displayFullModel); // vpMbKltMultiTracker::display(mapOfImages, mapOfCameraPoses, // mapOfCameraParameters, col, thickness, displayFullModel); // Display only features for KLT trackers (not the model) - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { - std::map *>::const_iterator - it_img = mapOfImages.find(it_klt->first); + std::map *>::const_iterator it_img = mapOfImages.find(it_klt->first); if (it_img != mapOfImages.end()) { - for (std::list::const_iterator it_pts = - it_klt->second->kltPolygons.begin(); + for (std::list::const_iterator it_pts = it_klt->second->kltPolygons.begin(); it_pts != it_klt->second->kltPolygons.end(); ++it_pts) { vpMbtDistanceKltPoints *kltpoly = *it_pts; - if (displayFeatures && kltpoly->hasEnoughPoints() && - kltpoly->isTracked() && kltpoly->polygon->isVisible()) { + if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) { kltpoly->displayPrimitive(*(it_img->second)); } } - for (std::list::const_iterator it_cyl = - it_klt->second->kltCylinders.begin(); + for (std::list::const_iterator it_cyl = it_klt->second->kltCylinders.begin(); it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl; - if (displayFeatures && kltPolyCylinder->isTracked() && - kltPolyCylinder->hasEnoughPoints()) + if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) kltPolyCylinder->displayPrimitive(*(it_img->second)); } } @@ -666,43 +591,33 @@ void vpMbEdgeKltMultiTracker::display( \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbEdgeKltMultiTracker::display( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness, - const bool displayFullModel) +void vpMbEdgeKltMultiTracker::display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, + const vpColor &col, const unsigned int thickness, const bool displayFullModel) { - vpMbEdgeMultiTracker::display(mapOfImages, mapOfCameraPoses, - mapOfCameraParameters, col, thickness, - displayFullModel); + vpMbEdgeMultiTracker::display(mapOfImages, mapOfCameraPoses, mapOfCameraParameters, col, thickness, displayFullModel); // vpMbKltMultiTracker::display(mapOfImages, mapOfCameraPoses, // mapOfCameraParameters, col, thickness, displayFullModel); // Display only features for KLT trackers (not the model) - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { - std::map *>::const_iterator it_img = - mapOfImages.find(it_klt->first); + std::map *>::const_iterator it_img = mapOfImages.find(it_klt->first); if (it_img != mapOfImages.end()) { - for (std::list::const_iterator it_pts = - it_klt->second->kltPolygons.begin(); + for (std::list::const_iterator it_pts = it_klt->second->kltPolygons.begin(); it_pts != it_klt->second->kltPolygons.end(); ++it_pts) { vpMbtDistanceKltPoints *kltpoly = *it_pts; - if (displayFeatures && kltpoly->hasEnoughPoints() && - kltpoly->isTracked() && kltpoly->polygon->isVisible()) { + if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) { kltpoly->displayPrimitive(*(it_img->second)); } } - for (std::list::const_iterator it_cyl = - it_klt->second->kltCylinders.begin(); + for (std::list::const_iterator it_cyl = it_klt->second->kltCylinders.begin(); it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl; - if (displayFeatures && kltPolyCylinder->isTracked() && - kltPolyCylinder->hasEnoughPoints()) + if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) kltPolyCylinder->displayPrimitive(*(it_img->second)); } } @@ -724,11 +639,7 @@ std::vector vpMbEdgeKltMultiTracker::getCameraNames() const \param camera : Copy of the camera parameters used by the tracker. */ -void vpMbEdgeKltMultiTracker::getCameraParameters( - vpCameraParameters &camera) const -{ - camera = this->cam; -} +void vpMbEdgeKltMultiTracker::getCameraParameters(vpCameraParameters &camera) const { camera = this->cam; } /*! Get the camera parameters for the stereo cameras case. @@ -736,8 +647,7 @@ void vpMbEdgeKltMultiTracker::getCameraParameters( \param cam1 : Copy of the camera parameters for the first camera. \param cam2 : Copy of the camera parameters for the second camera. */ -void vpMbEdgeKltMultiTracker::getCameraParameters( - vpCameraParameters &cam1, vpCameraParameters &cam2) const +void vpMbEdgeKltMultiTracker::getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const { // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class vpMbEdgeMultiTracker::getCameraParameters(cam1, cam2); @@ -749,8 +659,7 @@ void vpMbEdgeKltMultiTracker::getCameraParameters( \param cameraName : Name of the camera. \param camera : Copy of the camera parameters. */ -void vpMbEdgeKltMultiTracker::getCameraParameters( - const std::string &cameraName, vpCameraParameters &camera) const +void vpMbEdgeKltMultiTracker::getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const { // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class vpMbEdgeMultiTracker::getCameraParameters(cameraName, camera); @@ -778,11 +687,9 @@ void vpMbEdgeKltMultiTracker::getCameraParameters( \param cameraName : Name of the desired camera. \return Clipping flags. */ -unsigned int -vpMbEdgeKltMultiTracker::getClipping(const std::string &cameraName) const +unsigned int vpMbEdgeKltMultiTracker::getClipping(const std::string &cameraName) const { - std::map::const_iterator it = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(cameraName); if (it != m_mapOfKltTrackers.end()) { // Return the clipping flags for m_mapOfKltTrackers as it should be the // same for the same camera in m_mapOfEdgeTrackers @@ -810,8 +717,7 @@ vpMbHiddenFaces &vpMbEdgeKltMultiTracker::getFaces() \return Reference to the face structure. */ -vpMbHiddenFaces & -vpMbEdgeKltMultiTracker::getEdgeFaces(const std::string &cameraName) +vpMbHiddenFaces &vpMbEdgeKltMultiTracker::getEdgeFaces(const std::string &cameraName) { return vpMbEdgeMultiTracker::getFaces(cameraName); } @@ -821,8 +727,7 @@ vpMbEdgeKltMultiTracker::getEdgeFaces(const std::string &cameraName) \return Reference a map of the face structure for each camera. */ -std::map > -vpMbEdgeKltMultiTracker::getEdgeFaces() const +std::map > vpMbEdgeKltMultiTracker::getEdgeFaces() const { return vpMbEdgeMultiTracker::getFaces(); } @@ -833,8 +738,7 @@ vpMbEdgeKltMultiTracker::getEdgeFaces() const \return Reference to the face structure. */ -vpMbHiddenFaces & -vpMbEdgeKltMultiTracker::getKltFaces(const std::string &cameraName) +vpMbHiddenFaces &vpMbEdgeKltMultiTracker::getKltFaces(const std::string &cameraName) { return vpMbKltMultiTracker::getFaces(cameraName); } @@ -844,8 +748,7 @@ vpMbEdgeKltMultiTracker::getKltFaces(const std::string &cameraName) \return Reference a map of the face structure for each camera. */ -std::map > -vpMbEdgeKltMultiTracker::getKltFaces() const +std::map > vpMbEdgeKltMultiTracker::getKltFaces() const { return vpMbKltMultiTracker::getFaces(); } @@ -864,8 +767,7 @@ unsigned int vpMbEdgeKltMultiTracker::getNbPolygon() const \return Number of polygons. */ -std::map -vpMbEdgeKltMultiTracker::getEdgeMultiNbPolygon() const +std::map vpMbEdgeKltMultiTracker::getEdgeMultiNbPolygon() const { return vpMbEdgeMultiTracker::getMultiNbPolygon(); } @@ -876,8 +778,7 @@ vpMbEdgeKltMultiTracker::getEdgeMultiNbPolygon() const \return Number of polygons. */ -std::map -vpMbEdgeKltMultiTracker::getKltMultiNbPolygon() const +std::map vpMbEdgeKltMultiTracker::getKltMultiNbPolygon() const { return vpMbKltMultiTracker::getMultiNbPolygon(); } @@ -888,8 +789,7 @@ vpMbEdgeKltMultiTracker::getKltMultiNbPolygon() const \param c1Mo : The camera pose for the first camera. \param c2Mo : The camera pose for the second camera. */ -void vpMbEdgeKltMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, - vpHomogeneousMatrix &c2Mo) const +void vpMbEdgeKltMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const { // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class vpMbEdgeMultiTracker::getPose(c1Mo, c2Mo); @@ -903,8 +803,7 @@ void vpMbEdgeKltMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, \param cameraName : The name of the camera. \param cMo_ : The camera pose for the specified camera. */ -void vpMbEdgeKltMultiTracker::getPose(const std::string &cameraName, - vpHomogeneousMatrix &cMo_) const +void vpMbEdgeKltMultiTracker::getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo_) const { // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class vpMbEdgeMultiTracker::getPose(cameraName, cMo_); @@ -915,8 +814,7 @@ void vpMbEdgeKltMultiTracker::getPose(const std::string &cameraName, \param mapOfCameraPoses : The map of camera poses for all the cameras. */ -void vpMbEdgeKltMultiTracker::getPose( - std::map &mapOfCameraPoses) const +void vpMbEdgeKltMultiTracker::getPose(std::map &mapOfCameraPoses) const { // Clear the map mapOfCameraPoses.clear(); @@ -928,8 +826,7 @@ void vpMbEdgeKltMultiTracker::getPose( void vpMbEdgeKltMultiTracker::init(const vpImage & /*I*/) { if (!modelInitialised) { - throw vpException(vpTrackingException::initializationError, - "model not initialized"); + throw vpException(vpTrackingException::initializationError, "model not initialized"); } } @@ -944,9 +841,8 @@ void vpMbEdgeKltMultiTracker::init(const vpImage & /*I*/) image used to display the help. This image may be used to show where to click. This functionality is only available if visp_io module is used. */ -void vpMbEdgeKltMultiTracker::initClick( - const vpImage &I, - const std::vector &points3D_list, const std::string &displayFile) +void vpMbEdgeKltMultiTracker::initClick(const vpImage &I, const std::vector &points3D_list, + const std::string &displayFile) { // Cannot use directly set pose for KLT as it is different than for the edge // case It moves the KLT points instead of detecting new KLT points @@ -985,8 +881,7 @@ void vpMbEdgeKltMultiTracker::initClick( \exception vpException::ioError : The file specified in \e initFile doesn't exist. */ -void vpMbEdgeKltMultiTracker::initClick(const vpImage &I, - const std::string &initFile, +void vpMbEdgeKltMultiTracker::initClick(const vpImage &I, const std::string &initFile, const bool displayHelp) { // Cannot use directly set pose for KLT as it is different than for the edge @@ -1033,20 +928,15 @@ void vpMbEdgeKltMultiTracker::initClick(const vpImage &I, \exception vpException::ioError : The file specified in \e initFile doesn't exist. */ -void vpMbEdgeKltMultiTracker::initClick(const vpImage &I1, - const vpImage &I2, - const std::string &initFile1, - const std::string &initFile2, - const bool displayHelp, - const bool firstCameraIsReference) +void vpMbEdgeKltMultiTracker::initClick(const vpImage &I1, const vpImage &I2, + const std::string &initFile1, const std::string &initFile2, + const bool displayHelp, const bool firstCameraIsReference) { - vpMbKltMultiTracker::initClick(I1, I2, initFile1, initFile2, displayHelp, - firstCameraIsReference); + vpMbKltMultiTracker::initClick(I1, I2, initFile1, initFile2, displayHelp, firstCameraIsReference); // Get c2Mo vpHomogeneousMatrix c2Mo; - std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); if (firstCameraIsReference) { ++it_klt; it_klt->second->getPose(c2Mo); @@ -1087,9 +977,8 @@ void vpMbEdgeKltMultiTracker::initClick(const vpImage &I1, exist. */ -void vpMbEdgeKltMultiTracker::initClick( - const std::map *> &mapOfImages, - const std::string &initFile, const bool displayHelp) +void vpMbEdgeKltMultiTracker::initClick(const std::map *> &mapOfImages, + const std::string &initFile, const bool displayHelp) { vpMbKltMultiTracker::initClick(mapOfImages, initFile, displayHelp); @@ -1130,10 +1019,9 @@ void vpMbEdgeKltMultiTracker::initClick( \exception vpException::ioError : The file specified in \e initFile doesn't exist. */ -void vpMbEdgeKltMultiTracker::initClick( - const std::map *> &mapOfImages, - const std::map &mapOfInitFiles, - const bool displayHelp) +void vpMbEdgeKltMultiTracker::initClick(const std::map *> &mapOfImages, + const std::map &mapOfInitFiles, + const bool displayHelp) { vpMbKltMultiTracker::initClick(mapOfImages, mapOfInitFiles, displayHelp); @@ -1147,22 +1035,16 @@ void vpMbEdgeKltMultiTracker::initClick( } #endif //#ifdef VISP_HAVE_MODULE_GUI -void vpMbEdgeKltMultiTracker::initCircle(const vpPoint &, const vpPoint &, - const vpPoint &, const double, - const int, const std::string &) +void vpMbEdgeKltMultiTracker::initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double, const int, + const std::string &) { - std::cerr - << "The method initCircle is not used in vpMbEdgeKltMultiTracker !" - << std::endl; + std::cerr << "The method initCircle is not used in vpMbEdgeKltMultiTracker !" << std::endl; } -void vpMbEdgeKltMultiTracker::initCylinder(const vpPoint &, const vpPoint &, - const double, const int, +void vpMbEdgeKltMultiTracker::initCylinder(const vpPoint &, const vpPoint &, const double, const int, const std::string &) { - std::cerr - << "The method initCylinder is not used in vpMbEdgeKltMultiTracker !" - << std::endl; + std::cerr << "The method initCylinder is not used in vpMbEdgeKltMultiTracker !" << std::endl; } void vpMbEdgeKltMultiTracker::initFaceFromCorners(vpMbtPolygon &) @@ -1193,17 +1075,15 @@ void vpMbEdgeKltMultiTracker::initFaceFromLines(vpMbtPolygon &) \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. + the rotation in thetaU parametrisation (see vpThetaUVector). + \param I : Input image + \param initFile : Path to the file containing the pose. */ -void vpMbEdgeKltMultiTracker::initFromPose(const vpImage &I, - const std::string &initFile) +void vpMbEdgeKltMultiTracker::initFromPose(const vpImage &I, const std::string &initFile) { // Monocular case only ! if (m_mapOfKltTrackers.size() > 1) { - throw vpException( - vpTrackingException::fatalError, - "This function can only be used for the monocular case !"); + throw vpException(vpTrackingException::fatalError, "This function can only be used for the monocular case !"); } char s[FILENAME_MAX]; @@ -1239,14 +1119,11 @@ void vpMbEdgeKltMultiTracker::initFromPose(const vpImage &I, \param I : Input image \param cMo_ : Pose matrix. */ -void vpMbEdgeKltMultiTracker::initFromPose(const vpImage &I, - const vpHomogeneousMatrix &cMo_) +void vpMbEdgeKltMultiTracker::initFromPose(const vpImage &I, const vpHomogeneousMatrix &cMo_) { // Monocular case only ! if (m_mapOfKltTrackers.size() > 1) { - throw vpException( - vpTrackingException::fatalError, - "This function can only be used for the monocular case !"); + throw vpException(vpTrackingException::fatalError, "This function can only be used for the monocular case !"); } this->cMo = cMo_; @@ -1260,8 +1137,7 @@ void vpMbEdgeKltMultiTracker::initFromPose(const vpImage &I, \param I : Input image \param cPo : Pose vector. */ -void vpMbEdgeKltMultiTracker::initFromPose(const vpImage &I, - const vpPoseVector &cPo) +void vpMbEdgeKltMultiTracker::initFromPose(const vpImage &I, const vpPoseVector &cPo) { vpHomogeneousMatrix _cMo(cPo); initFromPose(I, _cMo); @@ -1277,16 +1153,12 @@ void vpMbEdgeKltMultiTracker::initFromPose(const vpImage &I, \param firstCameraIsReference : If true, the first camera is the reference camera, otherwise it is the second one. */ -void vpMbEdgeKltMultiTracker::initFromPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, +void vpMbEdgeKltMultiTracker::initFromPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference) { - vpMbEdgeMultiTracker::initFromPose(I1, I2, c1Mo, c2Mo, - firstCameraIsReference); - vpMbKltMultiTracker::initFromPose(I1, I2, c1Mo, c2Mo, - firstCameraIsReference); + vpMbEdgeMultiTracker::initFromPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference); + vpMbKltMultiTracker::initFromPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference); } /*! @@ -1296,9 +1168,8 @@ void vpMbEdgeKltMultiTracker::initFromPose(const vpImage &I1, \param mapOfImages : Map of images. \param cMo_ : Pose matrix for the reference camera. */ -void vpMbEdgeKltMultiTracker::initFromPose( - const std::map *> &mapOfImages, - const vpHomogeneousMatrix &cMo_) +void vpMbEdgeKltMultiTracker::initFromPose(const std::map *> &mapOfImages, + const vpHomogeneousMatrix &cMo_) { vpMbEdgeMultiTracker::initFromPose(mapOfImages, cMo_); vpMbKltMultiTracker::initFromPose(mapOfImages, cMo_); @@ -1310,24 +1181,22 @@ void vpMbEdgeKltMultiTracker::initFromPose( \param mapOfImages : Map of images. \param mapOfCameraPoses : Map of pose matrix. */ -void vpMbEdgeKltMultiTracker::initFromPose( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses) +void vpMbEdgeKltMultiTracker::initFromPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses) { vpMbEdgeMultiTracker::initFromPose(mapOfImages, mapOfCameraPoses); vpMbKltMultiTracker::initFromPose(mapOfImages, mapOfCameraPoses); } -unsigned int vpMbEdgeKltMultiTracker::initMbtTracking( - std::map *> &mapOfImages, - unsigned int lvl) +unsigned int +vpMbEdgeKltMultiTracker::initMbtTracking(std::map *> &mapOfImages, + unsigned int lvl) { vpMbEdgeTracker *edge = NULL; unsigned int nbrows = 0; m_factor.resize(0, false); - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { edge = it->second; @@ -1437,14 +1306,11 @@ void vpMbEdgeKltMultiTracker::loadConfigFile(const std::string &configFile) \sa loadConfigFile(const std::string &), vpXmlParser::cleanup() */ -void vpMbEdgeKltMultiTracker::loadConfigFile( - const std::string &configFile1, const std::string &configFile2, - const bool firstCameraIsReference) +void vpMbEdgeKltMultiTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2, + const bool firstCameraIsReference) { - vpMbEdgeMultiTracker::loadConfigFile(configFile1, configFile2, - firstCameraIsReference); - vpMbKltMultiTracker::loadConfigFile(configFile1, configFile2, - firstCameraIsReference); + vpMbEdgeMultiTracker::loadConfigFile(configFile1, configFile2, firstCameraIsReference); + vpMbKltMultiTracker::loadConfigFile(configFile1, configFile2, firstCameraIsReference); } /*! @@ -1460,8 +1326,7 @@ void vpMbEdgeKltMultiTracker::loadConfigFile( \sa loadConfigFile(const std::string &), vpXmlParser::cleanup() */ -void vpMbEdgeKltMultiTracker::loadConfigFile( - const std::map &mapOfConfigFiles) +void vpMbEdgeKltMultiTracker::loadConfigFile(const std::map &mapOfConfigFiles) { vpMbEdgeMultiTracker::loadConfigFile(mapOfConfigFiles); vpMbKltMultiTracker::loadConfigFile(mapOfConfigFiles); @@ -1492,8 +1357,7 @@ is not wrl or cao. \param verbose : verbose option to print additional information when loading CAO model files which include other CAO model files. */ -void vpMbEdgeKltMultiTracker::loadModel(const std::string &modelFile, - const bool verbose) +void vpMbEdgeKltMultiTracker::loadModel(const std::string &modelFile, const bool verbose) { vpMbEdgeMultiTracker::loadModel(modelFile, verbose); vpMbKltMultiTracker::loadModel(modelFile, verbose); @@ -1501,14 +1365,12 @@ void vpMbEdgeKltMultiTracker::loadModel(const std::string &modelFile, modelInitialised = true; } -void vpMbEdgeKltMultiTracker::postTracking( - std::map *> &mapOfImages, - const unsigned int lvl) +void vpMbEdgeKltMultiTracker::postTracking(std::map *> &mapOfImages, + const unsigned int lvl) { // MBT vpMbEdgeTracker *edge = NULL; - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { edge = it->second; @@ -1524,36 +1386,30 @@ void vpMbEdgeKltMultiTracker::postTracking( // Looking for new visible face bool newvisibleface = false; - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { edge = it->second; - edge->visibleFace(*mapOfImages[it->first], it->second->cMo, - newvisibleface); + edge->visibleFace(*mapOfImages[it->first], it->second->cMo, newvisibleface); } if (useScanLine) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { edge = it->second; edge->faces.computeClippedPolygons(it->second->cMo, it->second->cam); - edge->faces.computeScanLineRender(it->second->cam, - mapOfImages[it->first]->getWidth(), + edge->faces.computeScanLineRender(it->second->cam, mapOfImages[it->first]->getWidth(), mapOfImages[it->first]->getHeight()); } } - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { edge = it->second; edge->updateMovingEdge(*mapOfImages[it->first]); } - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { edge = it->second; edge->initMovingEdge(*mapOfImages[it->first], it->second->cMo); @@ -1582,10 +1438,8 @@ void vpMbEdgeKltMultiTracker::reinit(/*const vpImage& I */) model \param verbose : verbose option to print additional information when loading CAO model files which include other CAO model files. */ -void vpMbEdgeKltMultiTracker::reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbEdgeKltMultiTracker::reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose) { vpMbEdgeMultiTracker::reInitModel(I, cad_name, cMo_, verbose); vpMbKltMultiTracker::reInitModel(I, cad_name, cMo_, verbose); @@ -1604,18 +1458,13 @@ void vpMbEdgeKltMultiTracker::reInitModel(const vpImage &I, other CAO model files. \param firstCameraIsReference : If true, the first camera is the reference camera, otherwise it is the second one. */ -void vpMbEdgeKltMultiTracker::reInitModel(const vpImage &I1, - const vpImage &I2, - const std::string &cad_name, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, - const bool verbose, +void vpMbEdgeKltMultiTracker::reInitModel(const vpImage &I1, const vpImage &I2, + const std::string &cad_name, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const bool verbose, const bool firstCameraIsReference) { - vpMbEdgeMultiTracker::reInitModel(I1, I2, cad_name, c1Mo, c2Mo, verbose, - firstCameraIsReference); - vpMbKltMultiTracker::reInitModel(I1, I2, cad_name, c1Mo, c2Mo, verbose, - firstCameraIsReference); + vpMbEdgeMultiTracker::reInitModel(I1, I2, cad_name, c1Mo, c2Mo, verbose, firstCameraIsReference); + vpMbKltMultiTracker::reInitModel(I1, I2, cad_name, c1Mo, c2Mo, verbose, firstCameraIsReference); } /*! @@ -1628,16 +1477,13 @@ void vpMbEdgeKltMultiTracker::reInitModel(const vpImage &I1, additional information when loading CAO model files which include other CAO model files. */ -void vpMbEdgeKltMultiTracker::reInitModel( - const std::map *> &mapOfImages, - const std::string &cad_name, - const std::map &mapOfCameraPoses, - const bool verbose) +void vpMbEdgeKltMultiTracker::reInitModel(const std::map *> &mapOfImages, + const std::string &cad_name, + const std::map &mapOfCameraPoses, + const bool verbose) { - vpMbEdgeMultiTracker::reInitModel(mapOfImages, cad_name, mapOfCameraPoses, - verbose); - vpMbKltMultiTracker::reInitModel(mapOfImages, cad_name, mapOfCameraPoses, - verbose); + vpMbEdgeMultiTracker::reInitModel(mapOfImages, cad_name, mapOfCameraPoses, verbose); + vpMbKltMultiTracker::reInitModel(mapOfImages, cad_name, mapOfCameraPoses, verbose); } /*! @@ -1685,8 +1531,7 @@ void vpMbEdgeKltMultiTracker::setAngleDisappear(const double &a) \param camera : The new camera parameters. */ -void vpMbEdgeKltMultiTracker::setCameraParameters( - const vpCameraParameters &camera) +void vpMbEdgeKltMultiTracker::setCameraParameters(const vpCameraParameters &camera) { vpMbEdgeMultiTracker::setCameraParameters(camera); vpMbKltMultiTracker::setCameraParameters(camera); @@ -1702,14 +1547,11 @@ void vpMbEdgeKltMultiTracker::setCameraParameters( \param firstCameraIsReference : If true, the first camera is the reference, otherwise it is the second one. */ -void vpMbEdgeKltMultiTracker::setCameraParameters( - const vpCameraParameters &camera1, const vpCameraParameters &camera2, - const bool firstCameraIsReference) +void vpMbEdgeKltMultiTracker::setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2, + const bool firstCameraIsReference) { - vpMbEdgeMultiTracker::setCameraParameters(camera1, camera2, - firstCameraIsReference); - vpMbKltMultiTracker::setCameraParameters(camera1, camera2, - firstCameraIsReference); + vpMbEdgeMultiTracker::setCameraParameters(camera1, camera2, firstCameraIsReference); + vpMbKltMultiTracker::setCameraParameters(camera1, camera2, firstCameraIsReference); if (firstCameraIsReference) { this->cam = camera1; @@ -1724,8 +1566,7 @@ void vpMbEdgeKltMultiTracker::setCameraParameters( \param cameraName : Camera name. \param camera : The new camera parameters. */ -void vpMbEdgeKltMultiTracker::setCameraParameters( - const std::string &cameraName, const vpCameraParameters &camera) +void vpMbEdgeKltMultiTracker::setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera) { vpMbEdgeMultiTracker::setCameraParameters(cameraName, camera); vpMbKltMultiTracker::setCameraParameters(cameraName, camera); @@ -1746,8 +1587,7 @@ void vpMbEdgeKltMultiTracker::setCameraParameters( vpMbEdgeMultiTracker::setCameraParameters(mapOfCameraParameters); vpMbKltMultiTracker::setCameraParameters(mapOfCameraParameters); - for (std::map::const_iterator it = - mapOfCameraParameters.begin(); + for (std::map::const_iterator it = mapOfCameraParameters.begin(); it != mapOfCameraParameters.end(); ++it) { if (it->first == m_referenceCameraName) { this->cam = it->second; @@ -1763,17 +1603,13 @@ void vpMbEdgeKltMultiTracker::setCameraParameters( \param cameraTransformationMatrix : Camera transformation matrix between the current and the reference camera. */ -void vpMbEdgeKltMultiTracker::setCameraTransformationMatrix( - const std::string &cameraName, - const vpHomogeneousMatrix &cameraTransformationMatrix) +void vpMbEdgeKltMultiTracker::setCameraTransformationMatrix(const std::string &cameraName, + const vpHomogeneousMatrix &cameraTransformationMatrix) { - vpMbEdgeMultiTracker::setCameraTransformationMatrix( - cameraName, cameraTransformationMatrix); - vpMbKltMultiTracker::setCameraTransformationMatrix( - cameraName, cameraTransformationMatrix); + vpMbEdgeMultiTracker::setCameraTransformationMatrix(cameraName, cameraTransformationMatrix); + vpMbKltMultiTracker::setCameraTransformationMatrix(cameraName, cameraTransformationMatrix); - std::map::iterator it = - m_mapOfCameraTransformationMatrix.find(cameraName); + std::map::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName); if (it != m_mapOfCameraTransformationMatrix.end()) { it->second = cameraTransformationMatrix; } else { @@ -1789,13 +1625,10 @@ void vpMbEdgeKltMultiTracker::setCameraTransformationMatrix( \param mapOfTransformationMatrix : map of camera transformation matrices. */ void vpMbEdgeKltMultiTracker::setCameraTransformationMatrix( - const std::map - &mapOfTransformationMatrix) + const std::map &mapOfTransformationMatrix) { - vpMbEdgeMultiTracker::setCameraTransformationMatrix( - mapOfTransformationMatrix); - vpMbKltMultiTracker::setCameraTransformationMatrix( - mapOfTransformationMatrix); + vpMbEdgeMultiTracker::setCameraTransformationMatrix(mapOfTransformationMatrix); + vpMbKltMultiTracker::setCameraTransformationMatrix(mapOfTransformationMatrix); m_mapOfCameraTransformationMatrix = mapOfTransformationMatrix; } @@ -1821,8 +1654,7 @@ void vpMbEdgeKltMultiTracker::setClipping(const unsigned int &flags) \param cameraName : Camera to set the clipping. \param flags : New clipping flags. */ -void vpMbEdgeKltMultiTracker::setClipping(const std::string &cameraName, - const unsigned int &flags) +void vpMbEdgeKltMultiTracker::setClipping(const std::string &cameraName, const unsigned int &flags) { // Here, we do not change the general clipping flag vpMbEdgeMultiTracker::setClipping(cameraName, flags); @@ -1878,8 +1710,7 @@ void vpMbEdgeKltMultiTracker::setFarClippingDistance(const double &dist) \param cameraName : Camera to set the far clipping. \param dist : Far clipping value. */ -void vpMbEdgeKltMultiTracker::setFarClippingDistance( - const std::string &cameraName, const double &dist) +void vpMbEdgeKltMultiTracker::setFarClippingDistance(const std::string &cameraName, const double &dist) { vpMbEdgeMultiTracker::setFarClippingDistance(cameraName, dist); vpMbKltMultiTracker::setFarClippingDistance(cameraName, dist); @@ -1895,8 +1726,7 @@ void vpMbEdgeKltMultiTracker::setFarClippingDistance( \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%). */ -void vpMbEdgeKltMultiTracker::setGoodNbRayCastingAttemptsRatio( - const double &ratio) +void vpMbEdgeKltMultiTracker::setGoodNbRayCastingAttemptsRatio(const double &ratio) { vpMbEdgeMultiTracker::setGoodNbRayCastingAttemptsRatio(ratio); vpMbKltMultiTracker::setGoodNbRayCastingAttemptsRatio(ratio); @@ -1911,8 +1741,7 @@ void vpMbEdgeKltMultiTracker::setGoodNbRayCastingAttemptsRatio( \param attempts Number of rays to be sent. */ -void vpMbEdgeKltMultiTracker::setNbRayCastingAttemptsForVisibility( - const unsigned int &attempts) +void vpMbEdgeKltMultiTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { vpMbEdgeMultiTracker::setNbRayCastingAttemptsForVisibility(attempts); vpMbKltMultiTracker::setNbRayCastingAttemptsForVisibility(attempts); @@ -1930,8 +1759,7 @@ void vpMbEdgeKltMultiTracker::setNbRayCastingAttemptsForVisibility( \sa setMinLineLengthThresh(), setMinPolygonAreaThresh() */ -void vpMbEdgeKltMultiTracker::setLod(const bool useLod, - const std::string &name) +void vpMbEdgeKltMultiTracker::setLod(const bool useLod, const std::string &name) { vpMbEdgeMultiTracker::setLod(useLod, name); vpMbKltMultiTracker::setLod(useLod, name); @@ -1949,9 +1777,7 @@ void vpMbEdgeKltMultiTracker::setLod(const bool useLod, \sa setMinLineLengthThresh(), setMinPolygonAreaThresh() */ -void vpMbEdgeKltMultiTracker::setLod(const bool useLod, - const std::string &cameraName, - const std::string &name) +void vpMbEdgeKltMultiTracker::setLod(const bool useLod, const std::string &cameraName, const std::string &name) { vpMbEdgeMultiTracker::setLod(useLod, cameraName, name); vpMbKltMultiTracker::setLod(useLod, cameraName, name); @@ -1966,8 +1792,7 @@ void vpMbEdgeKltMultiTracker::setLod(const bool useLod, \sa setLod(), setMinPolygonAreaThresh() */ -void vpMbEdgeKltMultiTracker::setMinLineLengthThresh( - const double minLineLengthThresh, const std::string &name) +void vpMbEdgeKltMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name) { vpMbEdgeMultiTracker::setMinLineLengthThresh(minLineLengthThresh, name); } @@ -1983,12 +1808,10 @@ void vpMbEdgeKltMultiTracker::setMinLineLengthThresh( \sa setLod(), setMinPolygonAreaThresh() */ -void vpMbEdgeKltMultiTracker::setMinLineLengthThresh( - const double minLineLengthThresh, const std::string &cameraName, - const std::string &name) +void vpMbEdgeKltMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &cameraName, + const std::string &name) { - vpMbEdgeMultiTracker::setMinLineLengthThresh(minLineLengthThresh, - cameraName, name); + vpMbEdgeMultiTracker::setMinLineLengthThresh(minLineLengthThresh, cameraName, name); } /*! @@ -1999,8 +1822,7 @@ void vpMbEdgeKltMultiTracker::setMinLineLengthThresh( \sa setLod(), setMinLineLengthThresh() */ -void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh( - const double minPolygonAreaThresh, const std::string &name) +void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name) { vpMbEdgeMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, name); vpMbKltMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, name); @@ -2016,14 +1838,11 @@ void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh( \sa setLod(), setMinLineLengthThresh() */ -void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh( - const double minPolygonAreaThresh, const std::string &cameraName, - const std::string &name) +void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName, + const std::string &name) { - vpMbEdgeMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, - cameraName, name); - vpMbKltMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, - cameraName, name); + vpMbEdgeMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, cameraName, name); + vpMbKltMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, cameraName, name); } /*! @@ -2043,8 +1862,7 @@ void vpMbEdgeKltMultiTracker::setNearClippingDistance(const double &dist) \param cameraName : Camera name to set the near clipping distance. \param dist : Near clipping value. */ -void vpMbEdgeKltMultiTracker::setNearClippingDistance( - const std::string &cameraName, const double &dist) +void vpMbEdgeKltMultiTracker::setNearClippingDistance(const std::string &cameraName, const double &dist) { vpMbEdgeMultiTracker::setNearClippingDistance(cameraName, dist); vpMbKltMultiTracker::setNearClippingDistance(cameraName, dist); @@ -2060,8 +1878,7 @@ void vpMbEdgeKltMultiTracker::setNearClippingDistance( Ogre rendering options) when Ogre visibility is enabled. By default, this functionality is turned off. */ -void vpMbEdgeKltMultiTracker::setOgreShowConfigDialog( - const bool showConfigDialog) +void vpMbEdgeKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog) { vpMbEdgeMultiTracker::setOgreShowConfigDialog(showConfigDialog); vpMbKltMultiTracker::setOgreShowConfigDialog(showConfigDialog); @@ -2078,34 +1895,28 @@ void vpMbEdgeKltMultiTracker::setOgreShowConfigDialog( void vpMbEdgeKltMultiTracker::setOgreVisibilityTest(const bool &v) { // Edge - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setOgreVisibilityTest(v); } #ifdef VISP_HAVE_OGRE - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { - it->second->faces.getOgreContext()->setWindowName( - "Multi Edge MBT Hybrid (" + it->first + ")"); + it->second->faces.getOgreContext()->setWindowName("Multi Edge MBT Hybrid (" + it->first + ")"); } #endif // KLT - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setOgreVisibilityTest(v); } #ifdef VISP_HAVE_OGRE - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { - it->second->faces.getOgreContext()->setWindowName( - "Multi KLT MBT Hybrid (" + it->first + ")"); + it->second->faces.getOgreContext()->setWindowName("Multi KLT MBT Hybrid (" + it->first + ")"); } #endif @@ -2117,8 +1928,7 @@ void vpMbEdgeKltMultiTracker::setOgreVisibilityTest(const bool &v) \param opt : Optimization method to use. */ -void vpMbEdgeKltMultiTracker::setOptimizationMethod( - const vpMbtOptimizationMethod &opt) +void vpMbEdgeKltMultiTracker::setOptimizationMethod(const vpMbtOptimizationMethod &opt) { vpMbEdgeMultiTracker::setOptimizationMethod(opt); vpMbKltMultiTracker::setOptimizationMethod(opt); @@ -2131,18 +1941,14 @@ void vpMbEdgeKltMultiTracker::setOptimizationMethod( \param I : image corresponding to the desired pose. \param cMo_ : Pose to affect. */ -void vpMbEdgeKltMultiTracker::setPose(const vpImage &I, - const vpHomogeneousMatrix &cMo_) +void vpMbEdgeKltMultiTracker::setPose(const vpImage &I, const vpHomogeneousMatrix &cMo_) { if (m_mapOfEdgeTrackers.size() != 1 || m_mapOfKltTrackers.size() != 1) { std::cerr << "This method requires only 1 camera !" << std::endl; } else { - std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.find(m_referenceCameraName); - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); - if (it_edge != m_mapOfEdgeTrackers.end() && - it_klt != m_mapOfKltTrackers.end()) { + std::map::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); + if (it_edge != m_mapOfEdgeTrackers.end() && it_klt != m_mapOfKltTrackers.end()) { it_edge->second->setPose(I, cMo_); it_klt->second->setPose(I, cMo_); @@ -2150,8 +1956,7 @@ void vpMbEdgeKltMultiTracker::setPose(const vpImage &I, c0Mo = this->cMo; ctTc0.eye(); } else { - std::cerr << "Cannot find the reference camera: " - << m_referenceCameraName << " !" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << " !" << std::endl; } } } @@ -2167,10 +1972,8 @@ void vpMbEdgeKltMultiTracker::setPose(const vpImage &I, \param firstCameraIsReference : if true, the first camera is the reference, otherwise it is the second one. */ -void vpMbEdgeKltMultiTracker::setPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, +void vpMbEdgeKltMultiTracker::setPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference) { vpMbEdgeMultiTracker::setPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference); @@ -2185,9 +1988,8 @@ void vpMbEdgeKltMultiTracker::setPose(const vpImage &I1, \param mapOfImages : Map of images. \param cMo_ : Pose to affect to the reference camera. */ -void vpMbEdgeKltMultiTracker::setPose( - const std::map *> &mapOfImages, - const vpHomogeneousMatrix &cMo_) +void vpMbEdgeKltMultiTracker::setPose(const std::map *> &mapOfImages, + const vpHomogeneousMatrix &cMo_) { vpMbEdgeMultiTracker::setPose(mapOfImages, cMo_); vpMbKltMultiTracker::setPose(mapOfImages, cMo_); @@ -2203,9 +2005,8 @@ void vpMbEdgeKltMultiTracker::setPose( \param mapOfImages : Map of images corresponding to the desired pose. \param mapOfCameraPoses : Map of poses to affect. */ -void vpMbEdgeKltMultiTracker::setPose( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses) +void vpMbEdgeKltMultiTracker::setPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses) { vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses); vpMbKltMultiTracker::setPose(mapOfImages, mapOfCameraPoses); @@ -2223,8 +2024,7 @@ void vpMbEdgeKltMultiTracker::setProjectionErrorComputation(const bool &flag) vpMbTracker::setProjectionErrorComputation(flag); // Set the flag for each camera - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { it->second->setProjectionErrorComputation(flag); } @@ -2235,8 +2035,7 @@ void vpMbEdgeKltMultiTracker::setProjectionErrorComputation(const bool &flag) \param referenceCameraName : Name of the reference camera. */ -void vpMbEdgeKltMultiTracker::setReferenceCameraName( - const std::string &referenceCameraName) +void vpMbEdgeKltMultiTracker::setReferenceCameraName(const std::string &referenceCameraName) { vpMbEdgeMultiTracker::setReferenceCameraName(referenceCameraName); vpMbKltMultiTracker::setReferenceCameraName(referenceCameraName); @@ -2266,9 +2065,7 @@ void vpMbEdgeKltMultiTracker::setThresholdAcceptation(const double th) void vpMbEdgeKltMultiTracker::testTracking() { - std::cerr - << "The method vpMbEdgeKltMultiTracker::testTracking is not used !" - << std::endl; + std::cerr << "The method vpMbEdgeKltMultiTracker::testTracking is not used !" << std::endl; } /*! @@ -2282,20 +2079,16 @@ void vpMbEdgeKltMultiTracker::track(const vpImage &I) { // Track only with reference camera // Get the reference camera parameters - std::map::const_iterator it_mbt = - m_mapOfEdgeTrackers.find(m_referenceCameraName); - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it_mbt = m_mapOfEdgeTrackers.find(m_referenceCameraName); + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); - if (it_mbt != m_mapOfEdgeTrackers.end() && - it_klt != m_mapOfKltTrackers.end()) { + if (it_mbt != m_mapOfEdgeTrackers.end() && it_klt != m_mapOfKltTrackers.end()) { std::map *> mapOfImages; mapOfImages[m_referenceCameraName] = &I; track(mapOfImages); } else { std::stringstream ss; - ss << "The reference camera: " << m_referenceCameraName - << " does not exist !"; + ss << "The reference camera: " << m_referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::fatalError, ss.str().c_str()); } @@ -2313,12 +2106,10 @@ void vpMbEdgeKltMultiTracker::track(const vpImage &I) \param I1 : The first image. \param I2 : The second image. */ -void vpMbEdgeKltMultiTracker::track(const vpImage &I1, - const vpImage &I2) +void vpMbEdgeKltMultiTracker::track(const vpImage &I1, const vpImage &I2) { if (m_mapOfEdgeTrackers.size() == 2 && m_mapOfKltTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it = m_mapOfKltTrackers.begin(); std::map *> mapOfImages; // Assume that the first image is the first name in alphabetic order mapOfImages[it->first] = &I1; @@ -2328,8 +2119,7 @@ void vpMbEdgeKltMultiTracker::track(const vpImage &I1, track(mapOfImages); } else { std::stringstream ss; - ss << "Require two cameras ! There are " << m_mapOfKltTrackers.size() - << " cameras !"; + ss << "Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !"; throw vpException(vpTrackingException::fatalError, ss.str().c_str()); } } @@ -2341,36 +2131,29 @@ void vpMbEdgeKltMultiTracker::track(const vpImage &I1, \param mapOfImages : Map of images. */ -void vpMbEdgeKltMultiTracker::track( - std::map *> &mapOfImages) +void vpMbEdgeKltMultiTracker::track(std::map *> &mapOfImages) { // Reset the projectionError projectionError = 90.0; // Check if there is an image for each camera // mbt - for (std::map::const_iterator it_edge = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) { - std::map *>::const_iterator - it_img = mapOfImages.find(it_edge->first); + std::map *>::const_iterator it_img = mapOfImages.find(it_edge->first); if (it_img == mapOfImages.end()) { - throw vpException(vpTrackingException::fatalError, - "Missing images for edge trackers !"); + throw vpException(vpTrackingException::fatalError, "Missing images for edge trackers !"); } } // klt - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { - std::map *>::const_iterator - it_img = mapOfImages.find(it_klt->first); + std::map *>::const_iterator it_img = mapOfImages.find(it_klt->first); if (it_img == mapOfImages.end()) { - throw vpException(vpTrackingException::fatalError, - "Missing images for KLT trackers !"); + throw vpException(vpTrackingException::fatalError, "Missing images for KLT trackers !"); } } @@ -2391,11 +2174,9 @@ void vpMbEdgeKltMultiTracker::track( } } -void vpMbEdgeKltMultiTracker::trackMovingEdges( - std::map *> &mapOfImages) +void vpMbEdgeKltMultiTracker::trackMovingEdges(std::map *> &mapOfImages) { - for (std::map::const_iterator it = - m_mapOfEdgeTrackers.begin(); + for (std::map::const_iterator it = m_mapOfEdgeTrackers.begin(); it != m_mapOfEdgeTrackers.end(); ++it) { vpMbEdgeTracker *edge = it->second; // Track moving edges diff --git a/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp b/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp index 27c446b845..0d7c5f3280 100644 --- a/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp +++ b/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp @@ -44,12 +44,10 @@ #include #include -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) vpMbEdgeKltTracker::vpMbEdgeKltTracker() - : thresholdKLT(2.), thresholdMBT(2.), m_maxIterKlt(30), w_mbt(), w_klt(), - m_error_hybrid(), m_w_hybrid() + : thresholdKLT(2.), thresholdMBT(2.), m_maxIterKlt(30), w_mbt(), w_klt(), m_error_hybrid(), m_w_hybrid() { computeCovariance = false; @@ -106,8 +104,7 @@ void vpMbEdgeKltTracker::init(const vpImage &I) \param I : image corresponding to the desired pose. \param cdMo : Pose to affect. */ -void vpMbEdgeKltTracker::setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo) +void vpMbEdgeKltTracker::setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo) { vpMbKltTracker::setPose(I, cdMo); @@ -151,8 +148,7 @@ unsigned int vpMbEdgeKltTracker::initMbtTracking(const unsigned int lvl) } unsigned int nbrow = 0; - for (std::list::iterator it = lines[lvl].begin(); - it != lines[lvl].end(); ++it) { + for (std::list::iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) { vpMbtDistanceLine *l = *it; if (l->isTracked()) { @@ -161,9 +157,8 @@ unsigned int vpMbEdgeKltTracker::initMbtTracking(const unsigned int lvl) } } - for (std::list::const_iterator it = - cylinders[lvl].begin(); - it != cylinders[lvl].end(); ++it) { + for (std::list::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end(); + ++it) { vpMbtDistanceCylinder *cy = *it; if (cy->isTracked()) { @@ -172,9 +167,7 @@ unsigned int vpMbEdgeKltTracker::initMbtTracking(const unsigned int lvl) } } - for (std::list::const_iterator it = - circles[lvl].begin(); - it != circles[lvl].end(); ++it) { + for (std::list::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) { vpMbtDistanceCircle *ci = *it; if (ci->isTracked()) { @@ -289,13 +282,11 @@ void vpMbEdgeKltTracker::loadConfigFile(const char *configFile) xmlp.setMaskBorder(maskBorder); try { - std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " - << std::endl; + 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."); + throw vpException(vpException::ioError, "problem to parse configuration file."); } vpCameraParameters camera; @@ -351,35 +342,29 @@ void vpMbEdgeKltTracker::loadConfigFile(const char *configFile) /*! Realize the post tracking operations. Mostly visibility tests */ -bool vpMbEdgeKltTracker::postTracking(const vpImage &I, - vpColVector &w_mbt, vpColVector &w_klt, +bool vpMbEdgeKltTracker::postTracking(const vpImage &I, vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl) { postTrackingMbt(w_mbt, lvl); if (displayFeatures) { if (lvl == 0) { - for (std::list::const_iterator it = - lines[lvl].begin(); - it != lines[lvl].end(); ++it) { + for (std::list::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) { vpMbtDistanceLine *l = *it; if (l->isVisible() && l->isTracked()) { l->displayMovingEdges(I); } } - for (std::list::const_iterator it = - cylinders[lvl].begin(); - it != cylinders[lvl].end(); ++it) { + for (std::list::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end(); + ++it) { vpMbtDistanceCylinder *cy = *it; // A cylinder is always visible: #FIXME AY: Still valid? if (cy->isTracked()) cy->displayMovingEdges(I); } - for (std::list::const_iterator it = - circles[lvl].begin(); - it != circles[lvl].end(); ++it) { + for (std::list::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) { vpMbtDistanceCircle *ci = *it; if (ci->isVisible() && ci->isTracked()) { ci->displayMovingEdges(I); @@ -420,8 +405,7 @@ bool vpMbEdgeKltTracker::postTracking(const vpImage &I, \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) +void vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, const unsigned int lvl) { if (lvl >= scales.size() || !scales[lvl]) { throw vpException(vpException::dimensionError, "_lvl not used."); @@ -429,8 +413,7 @@ void vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, unsigned int n = 0; vpMbtDistanceLine *l; - for (std::list::const_iterator it = lines[lvl].begin(); - it != lines[lvl].end(); ++it) { + for (std::list::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) { if ((*it)->isTracked()) { l = *it; unsigned int indexLine = 0; @@ -470,9 +453,8 @@ void vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, // Same thing with cylinders as with lines vpMbtDistanceCylinder *cy; - for (std::list::const_iterator it = - cylinders[lvl].begin(); - it != cylinders[lvl].end(); ++it) { + for (std::list::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end(); + ++it) { if ((*it)->isTracked()) { cy = *it; double wmean = 0; @@ -537,9 +519,7 @@ void vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, // Same thing with circles as with lines vpMbtDistanceCircle *ci; - for (std::list::const_iterator it = - circles[lvl].begin(); - it != circles[lvl].end(); ++it) { + for (std::list::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) { if ((*it)->isTracked()) { ci = *it; double wmean = 0; @@ -586,17 +566,14 @@ void vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, \param nbrow : Size of the features (Edge). \param lvl : level of the pyramid. */ -void vpMbEdgeKltTracker::computeVVS(const vpImage &I, - const unsigned int &nbInfos, - unsigned int &nbrow, +void vpMbEdgeKltTracker::computeVVS(const vpImage &I, const unsigned int &nbInfos, unsigned int &nbrow, const unsigned int lvl) { vpColVector factor; nbrow = trackFirstLoop(I, factor, lvl); if (nbrow < 4 && nbInfos < 4) { - throw vpTrackingException(vpTrackingException::notEnoughPointError, - "Error: not enough features"); + throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features"); } else if (nbrow < 4) nbrow = 0; @@ -676,39 +653,29 @@ void vpMbEdgeKltTracker::computeVVS(const vpImage &I, if (nbInfos >= 4) { unsigned int shift = 0; - for (std::list::const_iterator it = - vpMbKltTracker::kltPolygons.begin(); + for (std::list::const_iterator it = vpMbKltTracker::kltPolygons.begin(); it != vpMbKltTracker::kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *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); + 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(); } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *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); + 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); + kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL); } catch (...) { - throw vpTrackingException(vpTrackingException::fatalError, - "Cannot compute interaction matrix"); + throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix"); } shift += 2 * kltPolyCylinder->getCurrentNumberPoints(); @@ -736,8 +703,7 @@ void vpMbEdgeKltTracker::computeVVS(const vpImage &I, } bool reStartFromLastIncrement = false; - computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, - cMoPrev, mu, reStartFromLastIncrement, + computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement, &m_w_prev); if (reStartFromLastIncrement) { ctTc0 = ctTc0_Prev; @@ -807,8 +773,7 @@ void vpMbEdgeKltTracker::computeVVS(const vpImage &I, residu = sqrt(num / den); - computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, - m_error_hybrid, m_error_prev, LTR, mu, v, + computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v, &m_w_hybrid, &m_w_prev); cMoPrev = cMo; @@ -820,15 +785,12 @@ void vpMbEdgeKltTracker::computeVVS(const vpImage &I, iter++; } - computeCovarianceMatrixVVS(isoJoIdentity, m_w_hybrid, cMoPrev, L_true, - LVJ_true, m_error_hybrid); + computeCovarianceMatrixVVS(isoJoIdentity, m_w_hybrid, cMoPrev, L_true, LVJ_true, m_error_hybrid); } void vpMbEdgeKltTracker::computeVVSInit() { - throw vpException( - vpException::fatalError, - "vpMbEdgeKltTracker::computeVVSInit() should not be called!"); + throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::computeVVSInit() should not be called!"); } void vpMbEdgeKltTracker::computeVVSInteractionMatrixAndResidu() @@ -889,10 +851,8 @@ void vpMbEdgeKltTracker::track(const vpImage &I) } } -unsigned int -vpMbEdgeKltTracker::trackFirstLoop(const vpImage &I, - vpColVector &factor, - const unsigned int lvl) +unsigned int vpMbEdgeKltTracker::trackFirstLoop(const vpImage &I, vpColVector &factor, + const unsigned int lvl) { vpMbtDistanceLine *l; vpMbtDistanceCylinder *cy; @@ -914,15 +874,14 @@ vpMbEdgeKltTracker::trackFirstLoop(const vpImage &I, factor = 1; unsigned int n = 0; - for (std::list::const_iterator it = lines[lvl].begin(); - it != lines[lvl].end(); ++it) { + for (std::list::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::const_iterator itindex = l->Lindex_polygon.begin(); - itindex != l->Lindex_polygon.end(); ++itindex) { + for (std::list::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; @@ -954,9 +913,8 @@ vpMbEdgeKltTracker::trackFirstLoop(const vpImage &I, } } - for (std::list::const_iterator it = - cylinders[lvl].begin(); - it != cylinders[lvl].end(); ++it) { + for (std::list::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end(); + ++it) { if ((*it)->isTracked()) { cy = *it; cy->computeInteractionMatrixError(cMo, I); @@ -987,9 +945,7 @@ vpMbEdgeKltTracker::trackFirstLoop(const vpImage &I, } } - for (std::list::const_iterator it = - circles[lvl].begin(); - it != circles[lvl].end(); ++it) { + for (std::list::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) { if ((*it)->isTracked()) { ci = *it; ci->computeInteractionMatrixError(cMo); @@ -1015,18 +971,15 @@ vpMbEdgeKltTracker::trackFirstLoop(const vpImage &I, return nbrow; } -void vpMbEdgeKltTracker::trackSecondLoop(const vpImage &I, - vpMatrix &L, vpColVector &error, - vpHomogeneousMatrix &cMo_, - const unsigned int lvl) +void vpMbEdgeKltTracker::trackSecondLoop(const vpImage &I, vpMatrix &L, vpColVector &error, + vpHomogeneousMatrix &cMo_, const unsigned int lvl) { vpMbtDistanceLine *l; vpMbtDistanceCylinder *cy; vpMbtDistanceCircle *ci; unsigned int n = 0; - for (std::list::const_iterator it = lines[lvl].begin(); - it != lines[lvl].end(); ++it) { + for (std::list::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) { if ((*it)->isTracked()) { l = *it; l->computeInteractionMatrixError(cMo_); @@ -1040,9 +993,8 @@ void vpMbEdgeKltTracker::trackSecondLoop(const vpImage &I, } } - for (std::list::const_iterator it = - cylinders[lvl].begin(); - it != cylinders[lvl].end(); ++it) { + for (std::list::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end(); + ++it) { if ((*it)->isTracked()) { cy = *it; cy->computeInteractionMatrixError(cMo_, I); @@ -1055,9 +1007,7 @@ void vpMbEdgeKltTracker::trackSecondLoop(const vpImage &I, n += cy->nbFeature; } } - for (std::list::const_iterator it = - circles[lvl].begin(); - it != circles[lvl].end(); ++it) { + for (std::list::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) { if ((*it)->isTracked()) { ci = *it; ci->computeInteractionMatrixError(cMo_); @@ -1119,8 +1069,7 @@ void vpMbEdgeKltTracker::initFaceFromLines(vpMbtPolygon &polygon) 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, +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); @@ -1136,8 +1085,7 @@ void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, \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, +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); @@ -1155,29 +1103,23 @@ void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, \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 &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, +void vpMbEdgeKltTracker::display(const vpImage &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::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } @@ -1186,22 +1128,17 @@ void vpMbEdgeKltTracker::display(const vpImage &I, } } - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; - if (displayFeatures && kltpoly->hasEnoughPoints() && - kltpoly->isTracked() && kltpoly->polygon->isVisible()) { + if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) { kltpoly->displayPrimitive(I); } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; - if (displayFeatures && kltPolyCylinder->isTracked() && - kltPolyCylinder->hasEnoughPoints()) + if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) kltPolyCylinder->displayPrimitive(I); } @@ -1222,29 +1159,23 @@ void vpMbEdgeKltTracker::display(const vpImage &I, \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 &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, +void vpMbEdgeKltTracker::display(const vpImage &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::const_iterator it = - lines[scaleLevel].begin(); - it != lines[scaleLevel].end(); ++it) { + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); + ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } @@ -1253,22 +1184,17 @@ void vpMbEdgeKltTracker::display(const vpImage &I, } } - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; - if (displayFeatures && kltpoly->hasEnoughPoints() && - kltpoly->isTracked() && kltpoly->polygon->isVisible()) { + if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) { kltpoly->displayPrimitive(I); } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; - if (displayFeatures && kltPolyCylinder->isTracked() && - kltPolyCylinder->hasEnoughPoints()) + if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) kltPolyCylinder->displayPrimitive(I); } @@ -1287,10 +1213,8 @@ void vpMbEdgeKltTracker::display(const vpImage &I, 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 &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbEdgeKltTracker::reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose) { reInitModel(I, cad_name.c_str(), cMo_, verbose); } @@ -1304,10 +1228,8 @@ void vpMbEdgeKltTracker::reInitModel(const vpImage &I, 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 &I, - const char *cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbEdgeKltTracker::reInitModel(const vpImage &I, const char *cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose) { // Reinit klt #if (VISP_HAVE_OPENCV_VERSION < 0x020408) @@ -1318,9 +1240,7 @@ void vpMbEdgeKltTracker::reInitModel(const vpImage &I, #endif // delete the Klt Polygon features - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; if (kltpoly != NULL) { delete kltpoly; @@ -1329,9 +1249,8 @@ void vpMbEdgeKltTracker::reInitModel(const vpImage &I, } kltPolygons.clear(); - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; if (kltPolyCylinder != NULL) { delete kltPolyCylinder; @@ -1342,9 +1261,7 @@ void vpMbEdgeKltTracker::reInitModel(const vpImage &I, // delete the structures used to display circles vpMbtDistanceCircle *ci; - for (std::list::const_iterator it = - circles_disp.begin(); - it != circles_disp.end(); ++it) { + for (std::list::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) { ci = *it; if (ci != NULL) { delete ci; @@ -1362,27 +1279,22 @@ void vpMbEdgeKltTracker::reInitModel(const vpImage &I, for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) { l = *it; if (l != NULL) delete l; l = NULL; } - for (std::list::const_iterator it = - cylinders[i].begin(); - it != cylinders[i].end(); ++it) { + for (std::list::const_iterator it = cylinders[i].begin(); it != cylinders[i].end(); + ++it) { cy = *it; if (cy != NULL) delete cy; cy = NULL; } - for (std::list::const_iterator it = - circles[i].begin(); - it != circles[i].end(); ++it) { + for (std::list::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) { ci = *it; if (ci != NULL) delete ci; diff --git a/modules/tracker/mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp b/modules/tracker/mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp index 1fff9c3eb2..b5c932a8dd 100644 --- a/modules/tracker/mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp +++ b/modules/tracker/mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp @@ -110,11 +110,9 @@ void vpMbtEdgeKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) bool klt_node = false; bool lod_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case vpMbtEdgeKltXmlParser::camera: { @@ -150,65 +148,42 @@ void vpMbtEdgeKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) } 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; + 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; + 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; + 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; + 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; + std::cout << "lod : min line length threshold : " << minLineLengthThreshold << " (default)" << std::endl; + std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << " (default)" << std::endl; } } diff --git a/modules/tracker/mbt/src/klt/vpMbKltMultiTracker.cpp b/modules/tracker/mbt/src/klt/vpMbKltMultiTracker.cpp index d277c200cd..4988282dff 100644 --- a/modules/tracker/mbt/src/klt/vpMbKltMultiTracker.cpp +++ b/modules/tracker/mbt/src/klt/vpMbKltMultiTracker.cpp @@ -45,8 +45,7 @@ #include -#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && \ - (VISP_HAVE_OPENCV_VERSION >= 0x020100) +#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100) #include #include @@ -56,9 +55,8 @@ Basic constructor */ vpMbKltMultiTracker::vpMbKltMultiTracker() - : m_mapOfCameraTransformationMatrix(), m_mapOfKltTrackers(), - m_referenceCameraName("Camera"), m_L_kltMulti(), m_error_kltMulti(), - m_w_kltMulti(), m_weightedError_kltMulti() + : m_mapOfCameraTransformationMatrix(), m_mapOfKltTrackers(), m_referenceCameraName("Camera"), m_L_kltMulti(), + m_error_kltMulti(), m_w_kltMulti(), m_weightedError_kltMulti() { m_mapOfKltTrackers["Camera"] = new vpMbKltTracker(); @@ -72,15 +70,12 @@ vpMbKltMultiTracker::vpMbKltMultiTracker() \param nbCameras : Number of cameras to use. */ vpMbKltMultiTracker::vpMbKltMultiTracker(const unsigned int nbCameras) - : m_mapOfCameraTransformationMatrix(), m_mapOfKltTrackers(), - m_referenceCameraName("Camera"), m_L_kltMulti(), m_error_kltMulti(), - m_w_kltMulti(), m_weightedError_kltMulti() + : m_mapOfCameraTransformationMatrix(), m_mapOfKltTrackers(), m_referenceCameraName("Camera"), m_L_kltMulti(), + m_error_kltMulti(), m_w_kltMulti(), m_weightedError_kltMulti() { if (nbCameras == 0) { - throw vpException( - vpTrackingException::fatalError, - "Cannot construct a vpMbkltMultiTracker with no camera !"); + throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbkltMultiTracker with no camera !"); } else if (nbCameras == 1) { m_mapOfKltTrackers["Camera"] = new vpMbKltTracker(); @@ -116,20 +111,15 @@ vpMbKltMultiTracker::vpMbKltMultiTracker(const unsigned int nbCameras) \param cameraNames : List of camera names. */ -vpMbKltMultiTracker::vpMbKltMultiTracker( - const std::vector &cameraNames) - : m_mapOfCameraTransformationMatrix(), m_mapOfKltTrackers(), - m_referenceCameraName("Camera"), m_L_kltMulti(), m_error_kltMulti(), - m_w_kltMulti(), m_weightedError_kltMulti() +vpMbKltMultiTracker::vpMbKltMultiTracker(const std::vector &cameraNames) + : m_mapOfCameraTransformationMatrix(), m_mapOfKltTrackers(), m_referenceCameraName("Camera"), m_L_kltMulti(), + m_error_kltMulti(), m_w_kltMulti(), m_weightedError_kltMulti() { if (cameraNames.empty()) { - throw vpException( - vpTrackingException::fatalError, - "Cannot construct a vpMbKltMultiTracker with no camera !"); + throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbKltMultiTracker with no camera !"); } - for (std::vector::const_iterator it = cameraNames.begin(); - it != cameraNames.end(); ++it) { + for (std::vector::const_iterator it = cameraNames.begin(); it != cameraNames.end(); ++it) { m_mapOfKltTrackers[*it] = new vpMbKltTracker(); } @@ -142,8 +132,7 @@ vpMbKltMultiTracker::vpMbKltMultiTracker( */ vpMbKltMultiTracker::~vpMbKltMultiTracker() { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { delete it->second; } @@ -157,12 +146,10 @@ vpMbKltMultiTracker::~vpMbKltMultiTracker() 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 vpMbKltMultiTracker::addCircle(const vpPoint &P1, const vpPoint &P2, - const vpPoint &P3, const double r, +void vpMbKltMultiTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->addCircle(P1, P2, P3, r, name); } @@ -186,16 +173,14 @@ void vpMbKltMultiTracker::computeVVS() unsigned int iter = 0; computeVVSInit(); - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { vpMbKltTracker *klt = it->second; klt->computeVVSInit(); } std::map mapOfVelocityTwist; - for (std::map::const_iterator it = - m_mapOfCameraTransformationMatrix.begin(); + for (std::map::const_iterator it = m_mapOfCameraTransformationMatrix.begin(); it != m_mapOfCameraTransformationMatrix.end(); ++it) { vpVelocityTwistMatrix cVo; cVo.buildFrom(it->second); @@ -206,8 +191,7 @@ void vpMbKltMultiTracker::computeVVS() computeVVSInteractionMatrixAndResidu(mapOfVelocityTwist); bool reStartFromLastIncrement = false; - computeVVSCheckLevenbergMarquardt(iter, m_error_kltMulti, error_prev, - cMoPrev, mu, reStartFromLastIncrement); + computeVVSCheckLevenbergMarquardt(iter, m_error_kltMulti, error_prev, cMoPrev, mu, reStartFromLastIncrement); if (reStartFromLastIncrement) { ctTc0 = ctTc0_Prev; } @@ -240,8 +224,7 @@ void vpMbKltMultiTracker::computeVVS() } } - computeVVSPoseEstimation(isoJoIdentity, iter, m_L_kltMulti, LTL, - m_weightedError_kltMulti, m_error_kltMulti, + computeVVSPoseEstimation(isoJoIdentity, iter, m_L_kltMulti, LTL, m_weightedError_kltMulti, m_error_kltMulti, error_prev, LTR, mu, v); cMoPrev = cMo; @@ -253,8 +236,7 @@ void vpMbKltMultiTracker::computeVVS() iter++; } - computeCovarianceMatrixVVS(isoJoIdentity, m_w_kltMulti, cMoPrev, L_true, - LVJ_true, m_error_kltMulti); + computeCovarianceMatrixVVS(isoJoIdentity, m_w_kltMulti, cMoPrev, L_true, LVJ_true, m_error_kltMulti); } void vpMbKltMultiTracker::computeVVSInit() @@ -279,8 +261,7 @@ void vpMbKltMultiTracker::computeVVSInteractionMatrixAndResidu( { unsigned int startIdx = 0; - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { vpMbKltTracker *klt = it->second; @@ -292,15 +273,13 @@ void vpMbKltMultiTracker::computeVVSInteractionMatrixAndResidu( klt->computeVVSInteractionMatrixAndResidu(); } else { vpHomogeneousMatrix c_curr_tTc_curr0 = - m_mapOfCameraTransformationMatrix[it->first] * cMo * - it->second->c0Mo.inverse(); + m_mapOfCameraTransformationMatrix[it->first] * cMo * it->second->c0Mo.inverse(); klt->ctTc0 = c_curr_tTc_curr0; klt->computeVVSInteractionMatrixAndResidu(); } m_error_kltMulti.insert(startIdx, klt->m_error_klt); - m_L_kltMulti.insert(klt->m_L_klt * mapOfVelocityTwist[it->first], - startIdx, 0); + m_L_kltMulti.insert(klt->m_L_klt * mapOfVelocityTwist[it->first], startIdx, 0); startIdx += klt->m_error_klt.getRows(); } @@ -309,8 +288,7 @@ void vpMbKltMultiTracker::computeVVSInteractionMatrixAndResidu( void vpMbKltMultiTracker::computeVVSWeights() { unsigned int startIdx = 0; - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { vpMbKltTracker *klt = it->second; klt->computeVVSWeights(klt->m_robust_klt, klt->m_error_klt, klt->m_w_klt); @@ -331,20 +309,15 @@ void vpMbKltMultiTracker::computeVVSWeights() \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbKltMultiTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, - const vpColor &col, - const unsigned int thickness, +void vpMbKltMultiTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { - std::map::const_iterator it = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName); if (it != m_mapOfKltTrackers.end()) { it->second->display(I, cMo_, cam_, col, thickness, displayFullModel); } else { - std::cerr << "Cannot find reference camera:" << m_referenceCameraName - << " !" << std::endl; + std::cerr << "Cannot find reference camera:" << m_referenceCameraName << " !" << std::endl; } } @@ -359,20 +332,15 @@ void vpMbKltMultiTracker::display(const vpImage &I, \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbKltMultiTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, - const vpColor &color, - const unsigned int thickness, +void vpMbKltMultiTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &cam_, const vpColor &color, const unsigned int thickness, const bool displayFullModel) { - std::map::const_iterator it = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName); if (it != m_mapOfKltTrackers.end()) { it->second->display(I, cMo_, cam_, color, thickness, displayFullModel); } else { - std::cerr << "Cannot find reference camera:" << m_referenceCameraName - << " !" << std::endl; + std::cerr << "Cannot find reference camera:" << m_referenceCameraName << " !" << std::endl; } } @@ -390,23 +358,20 @@ void vpMbKltMultiTracker::display(const vpImage &I, \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbKltMultiTracker::display( - const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, const vpCameraParameters &cam2, - const vpColor &color, const unsigned int thickness, - const bool displayFullModel) +void vpMbKltMultiTracker::display(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, + const vpCameraParameters &cam1, const vpCameraParameters &cam2, const vpColor &color, + const unsigned int thickness, const bool displayFullModel) { if (m_mapOfKltTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it = m_mapOfKltTrackers.begin(); it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel); ++it; it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel); } else { - std::cerr << "This display is only for the stereo case ! There are " - << m_mapOfKltTrackers.size() << " cameras !" << std::endl; + std::cerr << "This display is only for the stereo case ! There are " << m_mapOfKltTrackers.size() << " cameras !" + << std::endl; } } @@ -424,23 +389,20 @@ void vpMbKltMultiTracker::display( \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbKltMultiTracker::display( - const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, const vpCameraParameters &cam2, - const vpColor &color, const unsigned int thickness, - const bool displayFullModel) +void vpMbKltMultiTracker::display(const vpImage &I1, const vpImage &I2, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1, + const vpCameraParameters &cam2, const vpColor &color, const unsigned int thickness, + const bool displayFullModel) { if (m_mapOfKltTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it = m_mapOfKltTrackers.begin(); it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel); ++it; it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel); } else { - std::cerr << "This display is only for the stereo case ! There are " - << m_mapOfKltTrackers.size() << " cameras !" << std::endl; + std::cerr << "This display is only for the stereo case ! There are " << m_mapOfKltTrackers.size() << " cameras !" + << std::endl; } } @@ -455,31 +417,22 @@ void vpMbKltMultiTracker::display( \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbKltMultiTracker::display( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness, - const bool displayFullModel) +void vpMbKltMultiTracker::display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, + const vpColor &col, const unsigned int thickness, const bool displayFullModel) { // Display only for the given images - for (std::map *>::const_iterator - it_img = mapOfImages.begin(); + for (std::map *>::const_iterator it_img = mapOfImages.begin(); it_img != mapOfImages.end(); ++it_img) { - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(it_img->first); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(it_img->first); - std::map::const_iterator it_cam = - mapOfCameraParameters.find(it_img->first); - - if (it_klt != m_mapOfKltTrackers.end() && - it_camPose != mapOfCameraPoses.end() && + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(it_img->first); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first); + std::map::const_iterator it_cam = mapOfCameraParameters.find(it_img->first); + + if (it_klt != m_mapOfKltTrackers.end() && it_camPose != mapOfCameraPoses.end() && it_cam != mapOfCameraParameters.end()) { - it_klt->second->display(*it_img->second, it_camPose->second, - it_cam->second, col, thickness, - displayFullModel); + it_klt->second->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel); } else { std::cerr << "Missing elements ! " << std::endl; } @@ -497,31 +450,22 @@ void vpMbKltMultiTracker::display( \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbKltMultiTracker::display( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness, - const bool displayFullModel) +void vpMbKltMultiTracker::display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, + const vpColor &col, const unsigned int thickness, const bool displayFullModel) { // Display only for the given images - for (std::map *>::const_iterator it_img = - mapOfImages.begin(); + for (std::map *>::const_iterator it_img = mapOfImages.begin(); it_img != mapOfImages.end(); ++it_img) { - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(it_img->first); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(it_img->first); - std::map::const_iterator it_cam = - mapOfCameraParameters.find(it_img->first); - - if (it_klt != m_mapOfKltTrackers.end() && - it_camPose != mapOfCameraPoses.end() && + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(it_img->first); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first); + std::map::const_iterator it_cam = mapOfCameraParameters.find(it_img->first); + + if (it_klt != m_mapOfKltTrackers.end() && it_camPose != mapOfCameraPoses.end() && it_cam != mapOfCameraParameters.end()) { - it_klt->second->display(*it_img->second, it_camPose->second, - it_cam->second, col, thickness, - displayFullModel); + it_klt->second->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel); } else { std::cerr << "Missing elements ! " << std::endl; } @@ -537,8 +481,7 @@ std::vector vpMbKltMultiTracker::getCameraNames() const { std::vector cameraNames; - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { cameraNames.push_back(it_klt->first); } @@ -551,17 +494,14 @@ std::vector vpMbKltMultiTracker::getCameraNames() const \param camera : Copy of the camera parameters used by the tracker. */ -void vpMbKltMultiTracker::getCameraParameters( - vpCameraParameters &camera) const +void vpMbKltMultiTracker::getCameraParameters(vpCameraParameters &camera) const { // Get the reference camera parameters - std::map::const_iterator it = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName); if (it != m_mapOfKltTrackers.end()) { it->second->getCameraParameters(camera); } else { - std::cerr << "The reference camera name: " << m_referenceCameraName - << " does not exist !" << std::endl; + std::cerr << "The reference camera name: " << m_referenceCameraName << " does not exist !" << std::endl; } } @@ -571,19 +511,17 @@ void vpMbKltMultiTracker::getCameraParameters( \param cam1 : Copy of the camera parameters for the first camera. \param cam2 : Copy of the camera parameters for the second camera. */ -void vpMbKltMultiTracker::getCameraParameters(vpCameraParameters &cam1, - vpCameraParameters &cam2) const +void vpMbKltMultiTracker::getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const { if (m_mapOfKltTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it = m_mapOfKltTrackers.begin(); it->second->getCameraParameters(cam1); ++it; it->second->getCameraParameters(cam2); } else { - std::cerr << "Problem with the number of cameras ! There are " - << m_mapOfKltTrackers.size() << " cameras !" << std::endl; + std::cerr << "Problem with the number of cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !" + << std::endl; } } @@ -593,16 +531,13 @@ void vpMbKltMultiTracker::getCameraParameters(vpCameraParameters &cam1, \param cameraName : Name of the camera. \param camera : Copy of the camera parameters. */ -void vpMbKltMultiTracker::getCameraParameters( - const std::string &cameraName, vpCameraParameters &camera) const +void vpMbKltMultiTracker::getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const { - std::map::const_iterator it = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(cameraName); if (it != m_mapOfKltTrackers.end()) { it->second->getCameraParameters(camera); } else { - std::cerr << "The camera: " << cameraName << " does not exist !" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl; } } @@ -611,14 +546,12 @@ void vpMbKltMultiTracker::getCameraParameters( \param mapOfCameraParameters : Map of camera parameters. */ -void vpMbKltMultiTracker::getCameraParameters( - std::map &mapOfCameraParameters) const +void vpMbKltMultiTracker::getCameraParameters(std::map &mapOfCameraParameters) const { // Clear the input map mapOfCameraParameters.clear(); - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { vpCameraParameters cam_; it->second->getCameraParameters(cam_); @@ -633,11 +566,9 @@ void vpMbKltMultiTracker::getCameraParameters( \param cameraName : Name of the desired camera. \return Clipping flags. */ -unsigned int -vpMbKltMultiTracker::getClipping(const std::string &cameraName) const +unsigned int vpMbKltMultiTracker::getClipping(const std::string &cameraName) const { - std::map::const_iterator it = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(cameraName); if (it != m_mapOfKltTrackers.end()) { return it->second->getClipping(); } else { @@ -654,14 +585,12 @@ vpMbKltMultiTracker::getClipping(const std::string &cameraName) const */ vpMbHiddenFaces &vpMbKltMultiTracker::getFaces() { - std::map::const_iterator it = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName); if (it != m_mapOfKltTrackers.end()) { return it->second->getFaces(); } - std::cerr << "The reference camera: " << m_referenceCameraName - << " cannot be found !" << std::endl; + std::cerr << "The reference camera: " << m_referenceCameraName << " cannot be found !" << std::endl; return faces; } @@ -670,17 +599,14 @@ vpMbHiddenFaces &vpMbKltMultiTracker::getFaces() \return Reference to the face structure. */ -vpMbHiddenFaces & -vpMbKltMultiTracker::getFaces(const std::string &cameraName) +vpMbHiddenFaces &vpMbKltMultiTracker::getFaces(const std::string &cameraName) { - std::map::const_iterator it = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(cameraName); if (it != m_mapOfKltTrackers.end()) { return it->second->getFaces(); } - std::cerr << "The camera: " << cameraName << " cannot be found !" - << std::endl; + std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl; return faces; } @@ -689,12 +615,10 @@ vpMbKltMultiTracker::getFaces(const std::string &cameraName) \return Reference a map of the face structure for each camera. */ -std::map > -vpMbKltMultiTracker::getFaces() const +std::map > vpMbKltMultiTracker::getFaces() const { std::map > mapOfFaces; - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { mapOfFaces[it->first] = it->second->faces; } @@ -709,14 +633,12 @@ vpMbKltMultiTracker::getFaces() const */ std::list &vpMbKltMultiTracker::getFeaturesCircle() { - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); if (it_klt != m_mapOfKltTrackers.end()) { return it_klt->second->getFeaturesCircle(); } else { - std::cerr << "Cannot find reference camera: " << m_referenceCameraName - << " !" << std::endl; + std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl; } return circles_disp; @@ -728,11 +650,9 @@ std::list &vpMbKltMultiTracker::getFeaturesCircle() \param cameraName : Camera name. \return The address of the circle feature list. */ -std::list & -vpMbKltMultiTracker::getFeaturesCircle(const std::string &cameraName) +std::list &vpMbKltMultiTracker::getFeaturesCircle(const std::string &cameraName) { - std::map::const_iterator it = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(cameraName); if (it != m_mapOfKltTrackers.end()) { return it->second->getFeaturesCircle(); @@ -751,14 +671,12 @@ vpMbKltMultiTracker::getFeaturesCircle(const std::string &cameraName) */ std::list &vpMbKltMultiTracker::getFeaturesKlt() { - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); if (it_klt != m_mapOfKltTrackers.end()) { return it_klt->second->getFeaturesKlt(); } else { - std::cerr << "Cannot find reference camera: " << m_referenceCameraName - << " !" << std::endl; + std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl; } return kltPolygons; @@ -771,11 +689,9 @@ std::list &vpMbKltMultiTracker::getFeaturesKlt() \return The address of the Klt feature list. */ -std::list & -vpMbKltMultiTracker::getFeaturesKlt(const std::string &cameraName) +std::list &vpMbKltMultiTracker::getFeaturesKlt(const std::string &cameraName) { - std::map::const_iterator it = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(cameraName); if (it != m_mapOfKltTrackers.end()) { return it->second->getFeaturesKlt(); @@ -791,17 +707,14 @@ vpMbKltMultiTracker::getFeaturesKlt(const std::string &cameraName) \return The address of the cylinder feature list. */ -std::list & -vpMbKltMultiTracker::getFeaturesKltCylinder() +std::list &vpMbKltMultiTracker::getFeaturesKltCylinder() { - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); if (it_klt != m_mapOfKltTrackers.end()) { return it_klt->second->getFeaturesKltCylinder(); } else { - std::cerr << "Cannot find reference camera: " << m_referenceCameraName - << " !" << std::endl; + std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl; } return kltCylinders; @@ -813,11 +726,9 @@ vpMbKltMultiTracker::getFeaturesKltCylinder() \param cameraName : Camera name. \return The address of the cylinder feature list. */ -std::list & -vpMbKltMultiTracker::getFeaturesKltCylinder(const std::string &cameraName) +std::list &vpMbKltMultiTracker::getFeaturesKltCylinder(const std::string &cameraName) { - std::map::const_iterator it = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(cameraName); if (it != m_mapOfKltTrackers.end()) { return it->second->getFeaturesKltCylinder(); @@ -836,13 +747,11 @@ vpMbKltMultiTracker::getFeaturesKltCylinder(const std::string &cameraName) \return the list of KLT points through vpKltOpencv for each camera. */ -std::map > -vpMbKltMultiTracker::getKltImagePoints() const +std::map > vpMbKltMultiTracker::getKltImagePoints() const { std::map > mapOfFeatures; - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { mapOfFeatures[it->first] = it->second->getKltImagePoints(); } @@ -859,13 +768,11 @@ vpMbKltMultiTracker::getKltImagePoints() const \return the list of KLT points and their id through vpKltOpencv for each camera. */ -std::map > -vpMbKltMultiTracker::getKltImagePointsWithId() const +std::map > vpMbKltMultiTracker::getKltImagePointsWithId() const { std::map > mapOfFeatures; - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { mapOfFeatures[it->first] = it->second->getKltImagePointsWithId(); } @@ -882,8 +789,7 @@ std::map vpMbKltMultiTracker::getKltOpencv() const { std::map mapOfKltOpenCVTracker; - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { mapOfKltOpenCVTracker[it->first] = it->second->getKltOpencv(); } @@ -897,13 +803,11 @@ std::map vpMbKltMultiTracker::getKltOpencv() const \return The list of KLT points through vpKltOpencv. */ #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) -std::map > -vpMbKltMultiTracker::getKltPoints() const +std::map > vpMbKltMultiTracker::getKltPoints() const { std::map > mapOfFeatures; - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { mapOfFeatures[it->first] = it->second->getKltPoints(); } @@ -915,8 +819,7 @@ std::map vpMbKltMultiTracker::getKltPoints() { std::map mapOfFeatures; - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { mapOfFeatures[it->first] = it->second->getKltPoints(); } @@ -934,8 +837,7 @@ std::map vpMbKltMultiTracker::getKltNbPoints() const { std::map mapOfFeatures; - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { mapOfFeatures[it->first] = it->second->getKltNbPoints(); } @@ -950,14 +852,12 @@ std::map vpMbKltMultiTracker::getKltNbPoints() const */ unsigned int vpMbKltMultiTracker::getNbPolygon() const { - std::map::const_iterator it = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName); if (it != m_mapOfKltTrackers.end()) { return it->second->getNbPolygon(); } - std::cerr << "The reference camera: " << m_referenceCameraName - << " cannot be found !" << std::endl; + std::cerr << "The reference camera: " << m_referenceCameraName << " cannot be found !" << std::endl; return 0; } @@ -967,12 +867,10 @@ unsigned int vpMbKltMultiTracker::getNbPolygon() const \return Number of polygons for the specified camera. */ -std::map -vpMbKltMultiTracker::getMultiNbPolygon() const +std::map vpMbKltMultiTracker::getMultiNbPolygon() const { std::map mapOfNbPolygons; - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { mapOfNbPolygons[it->first] = it->second->getNbPolygon(); } @@ -986,19 +884,16 @@ vpMbKltMultiTracker::getMultiNbPolygon() const \param c1Mo : The camera pose for the first camera. \param c2Mo : The camera pose for the second camera. */ -void vpMbKltMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, - vpHomogeneousMatrix &c2Mo) const +void vpMbKltMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const { if (m_mapOfKltTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it = m_mapOfKltTrackers.begin(); it->second->getPose(c1Mo); ++it; it->second->getPose(c2Mo); } else { - std::cerr << "Require two cameras ! There are " - << m_mapOfKltTrackers.size() << " cameras !" << std::endl; + std::cerr << "Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !" << std::endl; } } @@ -1010,16 +905,13 @@ void vpMbKltMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, \param cameraName : The name of the camera. \param cMo_ : The camera pose for the specified camera. */ -void vpMbKltMultiTracker::getPose(const std::string &cameraName, - vpHomogeneousMatrix &cMo_) const +void vpMbKltMultiTracker::getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo_) const { - std::map::const_iterator it = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(cameraName); if (it != m_mapOfKltTrackers.end()) { it->second->getPose(cMo_); } else { - std::cerr << "The camera: " << cameraName << " does not exist !" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl; } } @@ -1028,14 +920,12 @@ void vpMbKltMultiTracker::getPose(const std::string &cameraName, \param mapOfCameraPoses : The map of camera poses for all the cameras. */ -void vpMbKltMultiTracker::getPose( - std::map &mapOfCameraPoses) const +void vpMbKltMultiTracker::getPose(std::map &mapOfCameraPoses) const { // Clear the map mapOfCameraPoses.clear(); - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { vpHomogeneousMatrix cMo_; it->second->getPose(cMo_); @@ -1056,20 +946,16 @@ void vpMbKltMultiTracker::init(const vpImage & /*I*/) {} image used to display the help. This image may be used to show where to click. This functionality is only available if visp_io module is used. */ -void vpMbKltMultiTracker::initClick(const vpImage &I, - const std::vector &points3D_list, +void vpMbKltMultiTracker::initClick(const vpImage &I, const std::vector &points3D_list, const std::string &displayFile) { if (m_mapOfKltTrackers.empty()) { - throw vpException(vpTrackingException::initializationError, - "There is no camera !"); + throw vpException(vpTrackingException::initializationError, "There is no camera !"); } else if (m_mapOfKltTrackers.size() > 1) { - throw vpException(vpTrackingException::initializationError, - "There is more than one camera !"); + throw vpException(vpTrackingException::initializationError, "There is more than one camera !"); } else { // Get the vpMbKltTracker object for the reference camera name - std::map::const_iterator it = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName); if (it != m_mapOfKltTrackers.end()) { it->second->initClick(I, points3D_list, displayFile); it->second->getPose(cMo); @@ -1079,10 +965,8 @@ void vpMbKltMultiTracker::initClick(const vpImage &I, ctTc0.eye(); } else { std::stringstream ss; - ss << "Cannot initClick as the reference camera: " - << m_referenceCameraName << " does not exist !"; - throw vpException(vpTrackingException::initializationError, - ss.str().c_str()); + ss << "Cannot initClick as the reference camera: " << m_referenceCameraName << " does not exist !"; + throw vpException(vpTrackingException::initializationError, ss.str().c_str()); } } } @@ -1115,20 +999,16 @@ void vpMbKltMultiTracker::initClick(const vpImage &I, \exception vpException::ioError : The file specified in \e initFile doesn't exist. */ -void vpMbKltMultiTracker::initClick(const vpImage &I, - const std::string &initFile, +void vpMbKltMultiTracker::initClick(const vpImage &I, const std::string &initFile, const bool displayHelp) { if (m_mapOfKltTrackers.empty()) { - throw vpException(vpTrackingException::initializationError, - "There is no camera !"); + throw vpException(vpTrackingException::initializationError, "There is no camera !"); } else if (m_mapOfKltTrackers.size() > 1) { - throw vpException(vpTrackingException::initializationError, - "There is more than one camera !"); + throw vpException(vpTrackingException::initializationError, "There is more than one camera !"); } else { // Get the vpMbKltTracker object for the reference camera name - std::map::const_iterator it = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName); if (it != m_mapOfKltTrackers.end()) { it->second->initClick(I, initFile, displayHelp); it->second->getPose(cMo); @@ -1138,10 +1018,8 @@ void vpMbKltMultiTracker::initClick(const vpImage &I, ctTc0.eye(); } else { std::stringstream ss; - ss << "Cannot initClick as the reference camera: " - << m_referenceCameraName << " does not exist !"; - throw vpException(vpTrackingException::initializationError, - ss.str().c_str()); + ss << "Cannot initClick as the reference camera: " << m_referenceCameraName << " does not exist !"; + throw vpException(vpTrackingException::initializationError, ss.str().c_str()); } } } @@ -1167,10 +1045,12 @@ void vpMbKltMultiTracker::initClick(const vpImage &I, \param I2 : Input image for the second camera. \param initFile1 : File containing the coordinates of at least 4 3D points the user has to click in the image acquired by the first camera. This file - should have .init extension (ie teabox.init). \param initFile2 : File + should have .init extension (ie teabox.init). + \param initFile2 : File containing the coordinates of at least 4 3D points the user has to click in the image acquired by the second camera. This file should have .init - extension. \param displayHelp : Optionnal display of an image that should + extension. + \param displayHelp : Optionnal display of an image that should have the same generic name as the init file (ie teabox.ppm). This image may be used to show where to click. This functionality is only available if visp_io module is used. @@ -1181,16 +1061,12 @@ void vpMbKltMultiTracker::initClick(const vpImage &I, \exception vpException::ioError : The file specified in \e initFile doesn't exist. */ -void vpMbKltMultiTracker::initClick(const vpImage &I1, - const vpImage &I2, - const std::string &initFile1, - const std::string &initFile2, - const bool displayHelp, +void vpMbKltMultiTracker::initClick(const vpImage &I1, const vpImage &I2, + const std::string &initFile1, const std::string &initFile2, const bool displayHelp, const bool firstCameraIsReference) { if (m_mapOfKltTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it = m_mapOfKltTrackers.begin(); it->second->initClick(I1, initFile1, displayHelp); if (firstCameraIsReference) { @@ -1222,10 +1098,8 @@ void vpMbKltMultiTracker::initClick(const vpImage &I1, } } else { std::stringstream ss; - ss << "Cannot init click ! Require two cameras but there are " - << m_mapOfKltTrackers.size() << " cameras !"; - throw vpException(vpTrackingException::initializationError, - ss.str().c_str()); + ss << "Cannot init click ! Require two cameras but there are " << m_mapOfKltTrackers.size() << " cameras !"; + throw vpException(vpTrackingException::initializationError, ss.str().c_str()); } } @@ -1248,7 +1122,8 @@ void vpMbKltMultiTracker::initClick(const vpImage &I1, \param mapOfImages : Map of images. \param initFile : File containing the points where to click for the - reference camera. \param displayHelp : Optionnal display of an image that + reference camera. + \param displayHelp : Optionnal display of an image that should have the same generic name as the init file (ie teabox.ppm). This image may be used to show where to click. This functionality is only available if visp_io module is used. @@ -1257,15 +1132,13 @@ void vpMbKltMultiTracker::initClick(const vpImage &I1, exist. */ -void vpMbKltMultiTracker::initClick( - const std::map *> &mapOfImages, - const std::string &initFile, const bool displayHelp) +void vpMbKltMultiTracker::initClick(const std::map *> &mapOfImages, + const std::string &initFile, const bool displayHelp) { - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); if (it_klt != m_mapOfKltTrackers.end()) { - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); if (it_img != mapOfImages.end()) { it_klt->second->initClick(*it_img->second, initFile, displayHelp); @@ -1278,41 +1151,32 @@ void vpMbKltMultiTracker::initClick( ctTc0.eye(); // Set the pose for the others cameras - for (it_klt = m_mapOfKltTrackers.begin(); - it_klt != m_mapOfKltTrackers.end(); ++it_klt) { + for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { if (it_klt->first != m_referenceCameraName) { it_img = mapOfImages.find(it_klt->first); - std::map::const_iterator - it_camTrans = - m_mapOfCameraTransformationMatrix.find(it_klt->first); + std::map::const_iterator it_camTrans = + m_mapOfCameraTransformationMatrix.find(it_klt->first); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; it_klt->second->cMo = cCurrentMo; it_klt->second->init(*it_img->second); } else { std::stringstream ss; - ss << "Cannot init click ! Missing image for camera: " - << m_referenceCameraName << " !"; - throw vpException(vpTrackingException::initializationError, - ss.str().c_str()); + ss << "Cannot init click ! Missing image for camera: " << m_referenceCameraName << " !"; + throw vpException(vpTrackingException::initializationError, ss.str().c_str()); } } } } else { std::stringstream ss; - ss << "Cannot init click ! Missing image for camera: " - << m_referenceCameraName << " !"; - throw vpException(vpTrackingException::initializationError, - ss.str().c_str()); + ss << "Cannot init click ! Missing image for camera: " << m_referenceCameraName << " !"; + throw vpException(vpTrackingException::initializationError, ss.str().c_str()); } } else { std::stringstream ss; - ss << "Cannot init click ! The reference camera: " - << m_referenceCameraName << " does not exist !"; - throw vpException(vpTrackingException::initializationError, - ss.str().c_str()); + ss << "Cannot init click ! The reference camera: " << m_referenceCameraName << " does not exist !"; + throw vpException(vpTrackingException::initializationError, ss.str().c_str()); } } @@ -1338,52 +1202,43 @@ void vpMbKltMultiTracker::initClick( \param mapOfImages : Map of images. \param mapOfInitFiles : map of files containing the points where to click - for each camera. \param displayHelp : Optional display of an image (ie + for each camera. + \param displayHelp : Optional display of an image (ie teabox.ppm). This image may be used to show where to click. \exception vpException::ioError : The file specified in \e initFile doesn't exist. */ -void vpMbKltMultiTracker::initClick( - const std::map *> &mapOfImages, - const std::map &mapOfInitFiles, - const bool displayHelp) -{ - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_initFile = - mapOfInitFiles.find(m_referenceCameraName); - - if (it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && - it_initFile != mapOfInitFiles.end()) { - it_klt->second->initClick(*it_img->second, it_initFile->second, - displayHelp); +void vpMbKltMultiTracker::initClick(const std::map *> &mapOfImages, + const std::map &mapOfInitFiles, const bool displayHelp) +{ + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_initFile = mapOfInitFiles.find(m_referenceCameraName); + + if (it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) { + it_klt->second->initClick(*it_img->second, it_initFile->second, displayHelp); it_klt->second->getPose(cMo); c0Mo = this->cMo; ctTc0.eye(); } else { - throw vpException(vpTrackingException::initializationError, - "Cannot initClick for the reference camera !"); + throw vpException(vpTrackingException::initializationError, "Cannot initClick for the reference camera !"); } // Vector of missing initFile for cameras std::vector vectorOfMissingCameraPoses; // Set pose for the specified cameras - for (it_klt = m_mapOfKltTrackers.begin(); - it_klt != m_mapOfKltTrackers.end(); ++it_klt) { + for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { if (it_klt->first != m_referenceCameraName) { it_img = mapOfImages.find(it_klt->first); it_initFile = mapOfInitFiles.find(it_klt->first); - if (it_img != mapOfImages.end() && - it_initFile != mapOfInitFiles.end()) { + if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) { // InitClick for the current camera - it_klt->second->initClick(*it_img->second, it_initFile->second, - displayHelp); + it_klt->second->initClick(*it_img->second, it_initFile->second, displayHelp); } else { vectorOfMissingCameraPoses.push_back(it_klt->first); } @@ -1391,15 +1246,13 @@ void vpMbKltMultiTracker::initClick( } // Set pose for cameras that do not have an initFile - for (std::vector::const_iterator it1 = - vectorOfMissingCameraPoses.begin(); + for (std::vector::const_iterator it1 = vectorOfMissingCameraPoses.begin(); it1 != vectorOfMissingCameraPoses.end(); ++it1) { it_img = mapOfImages.find(*it1); std::map::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it1); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; m_mapOfKltTrackers[*it1]->cMo = cCurrentMo; m_mapOfKltTrackers[*it1]->init(*it_img->second); @@ -1408,8 +1261,7 @@ void vpMbKltMultiTracker::initClick( ss << "Missing image or missing camera transformation matrix ! Cannot " "set the pose for camera: " << (*it1) << " !"; - throw vpException(vpTrackingException::initializationError, - ss.str().c_str()); + throw vpException(vpTrackingException::initializationError, ss.str().c_str()); } } } @@ -1429,17 +1281,15 @@ void vpMbKltMultiTracker::initClick( \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. + the rotation in thetaU parametrisation (see vpThetaUVector). + \param I : Input image + \param initFile : Path to the file containing the pose. */ -void vpMbKltMultiTracker::initFromPose(const vpImage &I, - const std::string &initFile) +void vpMbKltMultiTracker::initFromPose(const vpImage &I, const std::string &initFile) { // Monocular case only ! if (m_mapOfKltTrackers.size() > 1) { - throw vpException( - vpTrackingException::fatalError, - "This function can only be used for the monocular case !"); + throw vpException(vpTrackingException::fatalError, "This function can only be used for the monocular case !"); } char s[FILENAME_MAX]; @@ -1470,11 +1320,9 @@ void vpMbKltMultiTracker::initFromPose(const vpImage &I, ctTc0.eye(); // Init for the reference camera - std::map::iterator it_ref = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::iterator it_ref = m_mapOfKltTrackers.find(m_referenceCameraName); if (it_ref == m_mapOfKltTrackers.end()) { - throw vpException(vpTrackingException::initializationError, - "Cannot find the reference camera !"); + throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !"); } it_ref->second->initFromPose(I, cMo); @@ -1486,19 +1334,16 @@ void vpMbKltMultiTracker::initFromPose(const vpImage &I, \param I : Input image \param cMo_ : Pose matrix. */ -void vpMbKltMultiTracker::initFromPose(const vpImage &I, - const vpHomogeneousMatrix &cMo_) +void vpMbKltMultiTracker::initFromPose(const vpImage &I, const vpHomogeneousMatrix &cMo_) { this->cMo = cMo_; c0Mo = cMo; ctTc0.eye(); // Init for the reference camera - std::map::iterator it_ref = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::iterator it_ref = m_mapOfKltTrackers.find(m_referenceCameraName); if (it_ref == m_mapOfKltTrackers.end()) { - throw vpException(vpTrackingException::initializationError, - "Cannot find the reference camera !"); + throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !"); } it_ref->second->initFromPose(I, cMo); @@ -1510,8 +1355,7 @@ void vpMbKltMultiTracker::initFromPose(const vpImage &I, \param I : Input image \param cPo : Pose vector. */ -void vpMbKltMultiTracker::initFromPose(const vpImage &I, - const vpPoseVector &cPo) +void vpMbKltMultiTracker::initFromPose(const vpImage &I, const vpPoseVector &cPo) { vpHomogeneousMatrix _cMo(cPo); initFromPose(I, _cMo); @@ -1527,15 +1371,12 @@ void vpMbKltMultiTracker::initFromPose(const vpImage &I, \param firstCameraIsReference : If true, the first camera is the reference camera, otherwise it is the second one. */ -void vpMbKltMultiTracker::initFromPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, +void vpMbKltMultiTracker::initFromPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference) { if (m_mapOfKltTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it = m_mapOfKltTrackers.begin(); it->second->initFromPose(I1, c1Mo); ++it; @@ -1552,8 +1393,7 @@ void vpMbKltMultiTracker::initFromPose(const vpImage &I1, ctTc0.eye(); } else { std::stringstream ss; - ss << "This method requires 2 cameras but there are " - << m_mapOfKltTrackers.size() << " cameras !"; + ss << "This method requires 2 cameras but there are " << m_mapOfKltTrackers.size() << " cameras !"; throw vpException(vpTrackingException::initializationError, ss.str()); } } @@ -1565,16 +1405,13 @@ void vpMbKltMultiTracker::initFromPose(const vpImage &I1, \param mapOfImages : Map of images. \param cMo_ : Pose matrix for the reference camera. */ -void vpMbKltMultiTracker::initFromPose( - const std::map *> &mapOfImages, - const vpHomogeneousMatrix &cMo_) +void vpMbKltMultiTracker::initFromPose(const std::map *> &mapOfImages, + const vpHomogeneousMatrix &cMo_) { - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); if (it_klt != m_mapOfKltTrackers.end()) { - std::map *>::const_iterator - it_img = mapOfImages.find(it_klt->first); + std::map *>::const_iterator it_img = mapOfImages.find(it_klt->first); if (it_img != mapOfImages.end()) { it_klt->second->initFromPose(*it_img->second, cMo_); @@ -1583,33 +1420,27 @@ void vpMbKltMultiTracker::initFromPose( c0Mo = this->cMo; ctTc0.eye(); - for (it_klt = m_mapOfKltTrackers.begin(); - it_klt != m_mapOfKltTrackers.end(); ++it_klt) { + for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { if (it_klt->first != m_referenceCameraName) { - std::map::const_iterator - it_camTrans = - m_mapOfCameraTransformationMatrix.find(it_klt->first); + std::map::const_iterator it_camTrans = + m_mapOfCameraTransformationMatrix.find(it_klt->first); it_img = mapOfImages.find(it_klt->first); - if (it_camTrans != m_mapOfCameraTransformationMatrix.end() && - it_img != mapOfImages.end()) { + if (it_camTrans != m_mapOfCameraTransformationMatrix.end() && it_img != mapOfImages.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; it_klt->second->initFromPose(*it_img->second, cCurrentMo); } else { - throw vpException( - vpTrackingException::initializationError, - "Cannot find camera transformation matrix or image !"); + throw vpException(vpTrackingException::initializationError, + "Cannot find camera transformation matrix or image !"); } } } } else { - throw vpException(vpTrackingException::initializationError, - "Cannot find image for reference camera !"); + throw vpException(vpTrackingException::initializationError, "Cannot find image for reference camera !"); } } else { std::stringstream ss; - ss << "Cannot find reference camera: " << m_referenceCameraName - << std::endl; + ss << "Cannot find reference camera: " << m_referenceCameraName << std::endl; throw vpException(vpTrackingException::initializationError, ss.str()); } } @@ -1620,36 +1451,30 @@ void vpMbKltMultiTracker::initFromPose( \param mapOfImages : Map of images. \param mapOfCameraPoses : Map of pose matrix. */ -void vpMbKltMultiTracker::initFromPose( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses) +void vpMbKltMultiTracker::initFromPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses) { // Set the reference cMo - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(m_referenceCameraName); - - if (it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && - it_camPose != mapOfCameraPoses.end()) { + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName); + + if (it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) { it_klt->second->initFromPose(*it_img->second, it_camPose->second); it_klt->second->getPose(cMo); c0Mo = this->cMo; ctTc0.eye(); } else { - throw vpException(vpTrackingException::initializationError, - "Cannot set pose for the reference camera !"); + throw vpException(vpTrackingException::initializationError, "Cannot set pose for the reference camera !"); } // Vector of missing pose matrices for cameras std::vector vectorOfMissingCameraPoses; // Set pose for the specified cameras - for (it_klt = m_mapOfKltTrackers.begin(); - it_klt != m_mapOfKltTrackers.end(); ++it_klt) { + for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { it_img = mapOfImages.find(it_klt->first); it_camPose = mapOfCameraPoses.find(it_klt->first); @@ -1661,15 +1486,13 @@ void vpMbKltMultiTracker::initFromPose( } } - for (std::vector::const_iterator it1 = - vectorOfMissingCameraPoses.begin(); + for (std::vector::const_iterator it1 = vectorOfMissingCameraPoses.begin(); it1 != vectorOfMissingCameraPoses.end(); ++it1) { it_img = mapOfImages.find(*it1); std::map::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it1); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; m_mapOfKltTrackers[*it1]->initFromPose(*it_img->second, cCurrentMo); } else { @@ -1677,8 +1500,7 @@ void vpMbKltMultiTracker::initFromPose( ss << "Missing image or missing camera transformation matrix ! Cannot " "set the pose for camera: " << (*it1) << " !"; - throw vpException(vpTrackingException::initializationError, - ss.str().c_str()); + throw vpException(vpTrackingException::initializationError, ss.str().c_str()); } } } @@ -1732,8 +1554,7 @@ not found or wrong format for the data). */ void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile) { - std::map::const_iterator it = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName); if (it != m_mapOfKltTrackers.end()) { // Load ConfigFile for reference camera it->second->loadConfigFile(configFile); @@ -1745,10 +1566,8 @@ void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile) this->angleDisappears = it->second->getAngleDisappear(); } else { std::stringstream ss; - ss << "The reference camera: " << m_referenceCameraName - << " does not exist !"; - throw vpException(vpTrackingException::initializationError, - ss.str().c_str()); + ss << "The reference camera: " << m_referenceCameraName << " does not exist !"; + throw vpException(vpTrackingException::initializationError, ss.str().c_str()); } } @@ -1768,13 +1587,11 @@ void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile) \sa loadConfigFile(const std::string &), vpXmlParser::cleanup() */ -void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile1, - const std::string &configFile2, +void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2, const bool firstCameraIsReference) { if (m_mapOfKltTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it = m_mapOfKltTrackers.begin(); it->second->loadConfigFile(configFile1); if (firstCameraIsReference) { @@ -1799,10 +1616,8 @@ void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile1, } } else { std::stringstream ss; - ss << "Cannot loadConfigFile. Require two cameras ! There are " - << m_mapOfKltTrackers.size() << " cameras !"; - throw vpException(vpTrackingException::initializationError, - ss.str().c_str()); + ss << "Cannot loadConfigFile. Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !"; + throw vpException(vpTrackingException::initializationError, ss.str().c_str()); } } @@ -1819,28 +1634,22 @@ void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile1, \sa loadConfigFile(const std::string &), vpXmlParser::cleanup() */ -void vpMbKltMultiTracker::loadConfigFile( - const std::map &mapOfConfigFiles) +void vpMbKltMultiTracker::loadConfigFile(const std::map &mapOfConfigFiles) { - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { - std::map::const_iterator it_config = - mapOfConfigFiles.find(it_klt->first); + std::map::const_iterator it_config = mapOfConfigFiles.find(it_klt->first); if (it_config != mapOfConfigFiles.end()) { it_klt->second->loadConfigFile(it_config->second); } else { std::stringstream ss; - ss << "Missing configuration file for camera: " << it_klt->first - << " !"; - throw vpException(vpTrackingException::initializationError, - ss.str().c_str()); + ss << "Missing configuration file for camera: " << it_klt->first << " !"; + throw vpException(vpTrackingException::initializationError, ss.str().c_str()); } } // Set the reference camera parameters - std::map::iterator it = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::iterator it = m_mapOfKltTrackers.find(m_referenceCameraName); if (it != m_mapOfKltTrackers.end()) { it->second->getCameraParameters(cam); @@ -1850,10 +1659,8 @@ void vpMbKltMultiTracker::loadConfigFile( this->angleDisappears = it->second->getAngleDisappear(); } else { std::stringstream ss; - ss << "The reference camera: " << m_referenceCameraName - << " does not exist !"; - throw vpException(vpTrackingException::initializationError, - ss.str().c_str()); + ss << "The reference camera: " << m_referenceCameraName << " does not exist !"; + throw vpException(vpTrackingException::initializationError, ss.str().c_str()); } } @@ -1882,11 +1689,9 @@ is not wrl or cao. \param verbose : verbose option to print additional information when loading CAO model files which include other CAO model files. */ -void vpMbKltMultiTracker::loadModel(const std::string &modelFile, - const bool verbose) +void vpMbKltMultiTracker::loadModel(const std::string &modelFile, const bool verbose) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->loadModel(modelFile, verbose); } @@ -1894,14 +1699,12 @@ void vpMbKltMultiTracker::loadModel(const std::string &modelFile, modelInitialised = true; } -void vpMbKltMultiTracker::preTracking( - std::map *> &mapOfImages) +void vpMbKltMultiTracker::preTracking(std::map *> &mapOfImages) { m_nbInfos = 0; m_nbFaceUsed = 0; - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { vpMbKltTracker *klt = it->second; @@ -1914,20 +1717,17 @@ void vpMbKltMultiTracker::preTracking( } } -void vpMbKltMultiTracker::postTracking( - std::map *> &mapOfImages) +void vpMbKltMultiTracker::postTracking(std::map *> &mapOfImages) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { vpMbKltTracker *klt = it->second; // Set the camera pose it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo; - if (klt->m_nbInfos > 0 && - klt->postTracking(*mapOfImages[it->first], klt->m_w_klt)) { + if (klt->m_nbInfos > 0 && klt->postTracking(*mapOfImages[it->first], klt->m_w_klt)) { klt->reinit(*mapOfImages[it->first]); // set ctTc0 to identity @@ -1956,23 +1756,19 @@ void vpMbKltMultiTracker::reinit(/* const vpImage& I*/) model \param verbose : verbose option to print additional information when loading CAO model files which include other CAO model files. */ -void vpMbKltMultiTracker::reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbKltMultiTracker::reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose) { if (m_mapOfKltTrackers.size() != 1) { std::stringstream ss; - ss << "This method requires exactly one camera, there are " - << m_mapOfKltTrackers.size() << " cameras !"; + ss << "This method requires exactly one camera, there are " << m_mapOfKltTrackers.size() << " cameras !"; throw vpException(vpTrackingException::fatalError, ss.str().c_str()); } firstInitialisation = true; modelInitialised = true; - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); if (it_klt != m_mapOfKltTrackers.end()) { it_klt->second->reInitModel(I, cad_name, cMo_, verbose); @@ -1982,8 +1778,7 @@ void vpMbKltMultiTracker::reInitModel(const vpImage &I, c0Mo = cMo; ctTc0.eye(); } else { - throw vpException(vpTrackingException::fatalError, - "Cannot reInitModel the reference camera !"); + throw vpException(vpTrackingException::fatalError, "Cannot reInitModel the reference camera !"); } } @@ -2000,17 +1795,13 @@ void vpMbKltMultiTracker::reInitModel(const vpImage &I, other CAO model files. \param firstCameraIsReference : If true, the first camera is the reference camera, otherwise it is the second one. */ -void vpMbKltMultiTracker::reInitModel(const vpImage &I1, - const vpImage &I2, - const std::string &cad_name, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, - const bool verbose, +void vpMbKltMultiTracker::reInitModel(const vpImage &I1, const vpImage &I2, + const std::string &cad_name, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const bool verbose, const bool firstCameraIsReference) { if (m_mapOfKltTrackers.size() == 2) { - std::map::const_iterator it_edge = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it_edge = m_mapOfKltTrackers.begin(); it_edge->second->reInitModel(I1, cad_name, c1Mo, verbose); @@ -2031,8 +1822,7 @@ void vpMbKltMultiTracker::reInitModel(const vpImage &I1, c0Mo = cMo; ctTc0.eye(); } else { - throw vpException(vpTrackingException::fatalError, - "This method requires exactly two cameras !"); + throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras !"); } } @@ -2046,23 +1836,18 @@ void vpMbKltMultiTracker::reInitModel(const vpImage &I1, additional information when loading CAO model files which include other CAO model files. */ -void vpMbKltMultiTracker::reInitModel( - const std::map *> &mapOfImages, - const std::string &cad_name, - const std::map &mapOfCameraPoses, - const bool verbose) -{ - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(m_referenceCameraName); - - if (it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && - it_camPose != mapOfCameraPoses.end()) { - it_klt->second->reInitModel(*it_img->second, cad_name, it_camPose->second, - verbose); +void vpMbKltMultiTracker::reInitModel(const std::map *> &mapOfImages, + const std::string &cad_name, + const std::map &mapOfCameraPoses, + const bool verbose) +{ + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName); + + if (it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) { + it_klt->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose); modelInitialised = true; // Set reference pose @@ -2071,39 +1856,32 @@ void vpMbKltMultiTracker::reInitModel( c0Mo = cMo; ctTc0.eye(); } else { - throw vpException(vpTrackingException::fatalError, - "Cannot reInitModel for reference camera !"); + throw vpException(vpTrackingException::fatalError, "Cannot reInitModel for reference camera !"); } std::vector vectorOfMissingCameras; - for (it_klt = m_mapOfKltTrackers.begin(); - it_klt != m_mapOfKltTrackers.end(); ++it_klt) { + for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { if (it_klt->first != m_referenceCameraName) { it_img = mapOfImages.find(it_klt->first); it_camPose = mapOfCameraPoses.find(it_klt->first); - if (it_img != mapOfImages.end() && - it_camPose != mapOfCameraPoses.end()) { - it_klt->second->reInitModel(*it_img->second, cad_name, - it_camPose->second, verbose); + if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) { + it_klt->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose); } else { vectorOfMissingCameras.push_back(it_klt->first); } } } - for (std::vector::const_iterator it = - vectorOfMissingCameras.begin(); - it != vectorOfMissingCameras.end(); ++it) { + for (std::vector::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end(); + ++it) { it_img = mapOfImages.find(*it); std::map::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; - m_mapOfKltTrackers[*it]->reInitModel(*it_img->second, cad_name, - cCurrentMo, verbose); + m_mapOfKltTrackers[*it]->reInitModel(*it_img->second, cad_name, cCurrentMo, verbose); } } } @@ -2118,8 +1896,7 @@ void vpMbKltMultiTracker::resetTracker() ctTc0.eye(); // Call resetTracker for all the cameras - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->resetTracker(); } @@ -2162,8 +1939,7 @@ void vpMbKltMultiTracker::setAngleAppear(const double &a) { vpMbTracker::setAngleAppear(a); - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setAngleAppear(a); } @@ -2182,8 +1958,7 @@ void vpMbKltMultiTracker::setAngleDisappear(const double &a) { vpMbTracker::setAngleDisappear(a); - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setAngleDisappear(a); } @@ -2194,18 +1969,14 @@ void vpMbKltMultiTracker::setAngleDisappear(const double &a) \param camera : The new camera parameters. */ -void vpMbKltMultiTracker::setCameraParameters( - const vpCameraParameters &camera) +void vpMbKltMultiTracker::setCameraParameters(const vpCameraParameters &camera) { if (m_mapOfKltTrackers.empty()) { - throw vpException(vpTrackingException::fatalError, - "There is no camera !"); + throw vpException(vpTrackingException::fatalError, "There is no camera !"); } else if (m_mapOfKltTrackers.size() > 1) { - throw vpException(vpTrackingException::fatalError, - "There is more than one camera !"); + throw vpException(vpTrackingException::fatalError, "There is more than one camera !"); } else { - std::map::const_iterator it = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName); if (it != m_mapOfKltTrackers.end()) { it->second->setCameraParameters(camera); @@ -2213,8 +1984,7 @@ void vpMbKltMultiTracker::setCameraParameters( this->cam = camera; } else { std::stringstream ss; - ss << "The reference camera: " << m_referenceCameraName - << " does not exist !"; + ss << "The reference camera: " << m_referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::fatalError, ss.str()); } } @@ -2228,16 +1998,13 @@ void vpMbKltMultiTracker::setCameraParameters( \param firstCameraIsReference : If true, the first camera is the reference, otherwise it is the second one. */ -void vpMbKltMultiTracker::setCameraParameters( - const vpCameraParameters &camera1, const vpCameraParameters &camera2, - const bool firstCameraIsReference) +void vpMbKltMultiTracker::setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2, + const bool firstCameraIsReference) { if (m_mapOfKltTrackers.empty()) { - throw vpException(vpTrackingException::fatalError, - "There is no camera !"); + throw vpException(vpTrackingException::fatalError, "There is no camera !"); } else if (m_mapOfKltTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it = m_mapOfKltTrackers.begin(); it->second->setCameraParameters(camera1); ++it; @@ -2250,8 +2017,7 @@ void vpMbKltMultiTracker::setCameraParameters( } } else { std::stringstream ss; - ss << "Require two cameras ! There are " << m_mapOfKltTrackers.size() - << " cameras !"; + ss << "Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !"; throw vpException(vpTrackingException::fatalError, ss.str()); } } @@ -2262,11 +2028,9 @@ void vpMbKltMultiTracker::setCameraParameters( \param cameraName : Camera name. \param camera : The new camera parameters. */ -void vpMbKltMultiTracker::setCameraParameters( - const std::string &cameraName, const vpCameraParameters &camera) +void vpMbKltMultiTracker::setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera) { - std::map::const_iterator it = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(cameraName); if (it != m_mapOfKltTrackers.end()) { it->second->setCameraParameters(camera); @@ -2285,14 +2049,11 @@ void vpMbKltMultiTracker::setCameraParameters( \param mapOfCameraParameters : Map of camera parameters. */ -void vpMbKltMultiTracker::setCameraParameters( - const std::map &mapOfCameraParameters) +void vpMbKltMultiTracker::setCameraParameters(const std::map &mapOfCameraParameters) { - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { - std::map::const_iterator it_cam = - mapOfCameraParameters.find(it_klt->first); + std::map::const_iterator it_cam = mapOfCameraParameters.find(it_klt->first); if (it_cam != mapOfCameraParameters.end()) { it_klt->second->setCameraParameters(it_cam->second); @@ -2315,12 +2076,10 @@ void vpMbKltMultiTracker::setCameraParameters( \param cameraTransformationMatrix : Camera transformation matrix between the current and the reference camera. */ -void vpMbKltMultiTracker::setCameraTransformationMatrix( - const std::string &cameraName, - const vpHomogeneousMatrix &cameraTransformationMatrix) +void vpMbKltMultiTracker::setCameraTransformationMatrix(const std::string &cameraName, + const vpHomogeneousMatrix &cameraTransformationMatrix) { - std::map::iterator it = - m_mapOfCameraTransformationMatrix.find(cameraName); + std::map::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName); if (it != m_mapOfCameraTransformationMatrix.end()) { it->second = cameraTransformationMatrix; } else { @@ -2338,19 +2097,16 @@ void vpMbKltMultiTracker::setCameraTransformationMatrix( \param mapOfTransformationMatrix : map of camera transformation matrices. */ void vpMbKltMultiTracker::setCameraTransformationMatrix( - const std::map - &mapOfTransformationMatrix) + const std::map &mapOfTransformationMatrix) { // Check if all cameras have a transformation matrix - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { std::map::const_iterator it_camTrans = mapOfTransformationMatrix.find(it_klt->first); if (it_camTrans == mapOfTransformationMatrix.end()) { - throw vpException(vpTrackingException::initializationError, - "Missing camera transformation matrix !"); + throw vpException(vpTrackingException::initializationError, "Missing camera transformation matrix !"); } } @@ -2368,8 +2124,7 @@ void vpMbKltMultiTracker::setClipping(const unsigned int &flags) { vpMbTracker::setClipping(flags); - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setClipping(flags); } @@ -2383,11 +2138,9 @@ void vpMbKltMultiTracker::setClipping(const unsigned int &flags) \param cameraName : Camera to set the clipping. \param flags : New clipping flags. */ -void vpMbKltMultiTracker::setClipping(const std::string &cameraName, - const unsigned int &flags) +void vpMbKltMultiTracker::setClipping(const std::string &cameraName, const unsigned int &flags) { - std::map::const_iterator it = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(cameraName); if (it != m_mapOfKltTrackers.end()) { it->second->setClipping(flags); } else { @@ -2404,8 +2157,7 @@ void vpMbKltMultiTracker::setCovarianceComputation(const bool &flag) { vpMbTracker::setCovarianceComputation(flag); - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setCovarianceComputation(flag); } @@ -2418,8 +2170,7 @@ void vpMbKltMultiTracker::setCovarianceComputation(const bool &flag) */ void vpMbKltMultiTracker::setDisplayFeatures(const bool displayF) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setDisplayFeatures(displayF); } @@ -2436,8 +2187,7 @@ void vpMbKltMultiTracker::setFarClippingDistance(const double &dist) { vpMbTracker::setFarClippingDistance(dist); - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setFarClippingDistance(dist); } @@ -2449,11 +2199,9 @@ void vpMbKltMultiTracker::setFarClippingDistance(const double &dist) \param cameraName : Camera to set the far clipping. \param dist : Far clipping value. */ -void vpMbKltMultiTracker::setFarClippingDistance( - const std::string &cameraName, const double &dist) +void vpMbKltMultiTracker::setFarClippingDistance(const std::string &cameraName, const double &dist) { - std::map::const_iterator it = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(cameraName); if (it != m_mapOfKltTrackers.end()) { it->second->setFarClippingDistance(dist); } else { @@ -2471,11 +2219,9 @@ void vpMbKltMultiTracker::setFarClippingDistance( \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%). */ -void vpMbKltMultiTracker::setGoodNbRayCastingAttemptsRatio( - const double &ratio) +void vpMbKltMultiTracker::setGoodNbRayCastingAttemptsRatio(const double &ratio) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setGoodNbRayCastingAttemptsRatio(ratio); } @@ -2490,11 +2236,9 @@ void vpMbKltMultiTracker::setGoodNbRayCastingAttemptsRatio( \param attempts Number of rays to be sent. */ -void vpMbKltMultiTracker::setNbRayCastingAttemptsForVisibility( - const unsigned int &attempts) +void vpMbKltMultiTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setNbRayCastingAttemptsForVisibility(attempts); } @@ -2508,8 +2252,7 @@ void vpMbKltMultiTracker::setNbRayCastingAttemptsForVisibility( */ void vpMbKltMultiTracker::setKltOpencv(const vpKltOpencv &t) { - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { it_klt->second->setKltOpencv(t); } @@ -2520,19 +2263,15 @@ void vpMbKltMultiTracker::setKltOpencv(const vpKltOpencv &t) \param mapOfOpenCVTrackers : Map of Klt trackers containing the new values. */ -void vpMbKltMultiTracker::setKltOpencv( - const std::map &mapOfOpenCVTrackers) +void vpMbKltMultiTracker::setKltOpencv(const std::map &mapOfOpenCVTrackers) { - for (std::map::const_iterator it_kltOpenCV = - mapOfOpenCVTrackers.begin(); + for (std::map::const_iterator it_kltOpenCV = mapOfOpenCVTrackers.begin(); it_kltOpenCV != mapOfOpenCVTrackers.end(); ++it_kltOpenCV) { - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(it_kltOpenCV->first); + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(it_kltOpenCV->first); if (it_klt != m_mapOfKltTrackers.end()) { it_klt->second->setKltOpencv(it_kltOpenCV->second); } else { - std::cerr << "The camera: " << it_kltOpenCV->first - << " does not exist !" << std::endl; + std::cerr << "The camera: " << it_kltOpenCV->first << " does not exist !" << std::endl; } } } @@ -2550,8 +2289,7 @@ void vpMbKltMultiTracker::setKltOpencv( */ void vpMbKltMultiTracker::setLod(const bool useLod, const std::string &name) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setLod(useLod, name); } @@ -2569,18 +2307,14 @@ void vpMbKltMultiTracker::setLod(const bool useLod, const std::string &name) \sa setMinLineLengthThresh(), setMinPolygonAreaThresh() */ -void vpMbKltMultiTracker::setLod(const bool useLod, - const std::string &cameraName, - const std::string &name) +void vpMbKltMultiTracker::setLod(const bool useLod, const std::string &cameraName, const std::string &name) { - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(cameraName); if (it_klt != m_mapOfKltTrackers.end()) { it_klt->second->setLod(useLod, name); } else { - std::cerr << "The camera: " << cameraName << " cannot be found !" - << std::endl; + std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl; } } @@ -2591,8 +2325,7 @@ void vpMbKltMultiTracker::setLod(const bool useLod, */ void vpMbKltMultiTracker::setKltMaskBorder(const unsigned int &e) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setKltMaskBorder(e); } @@ -2603,8 +2336,7 @@ void vpMbKltMultiTracker::setKltMaskBorder(const unsigned int &e) /*! Useless for KLT tracker. */ -void vpMbKltMultiTracker::setMinLineLengthThresh( - const double /*minLineLengthThresh*/, const std::string & /*name*/) +void vpMbKltMultiTracker::setMinLineLengthThresh(const double /*minLineLengthThresh*/, const std::string & /*name*/) { std::cerr << "Useless for KLT tracker !" << std::endl; } @@ -2617,11 +2349,9 @@ void vpMbKltMultiTracker::setMinLineLengthThresh( \sa setLod(), setMinLineLengthThresh() */ -void vpMbKltMultiTracker::setMinPolygonAreaThresh( - const double minPolygonAreaThresh, const std::string &name) +void vpMbKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name); } @@ -2637,18 +2367,15 @@ void vpMbKltMultiTracker::setMinPolygonAreaThresh( \sa setLod(), setMinLineLengthThresh() */ -void vpMbKltMultiTracker::setMinPolygonAreaThresh( - const double minPolygonAreaThresh, const std::string &cameraName, - const std::string &name) +void vpMbKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName, + const std::string &name) { - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(cameraName); if (it_klt != m_mapOfKltTrackers.end()) { it_klt->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name); } else { - std::cerr << "The camera: " << cameraName << " cannot be found !" - << std::endl; + std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl; } } @@ -2661,8 +2388,7 @@ void vpMbKltMultiTracker::setNearClippingDistance(const double &dist) { vpMbTracker::setNearClippingDistance(dist); - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setNearClippingDistance(dist); } @@ -2680,8 +2406,7 @@ void vpMbKltMultiTracker::setNearClippingDistance(const double &dist) */ void vpMbKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setOgreShowConfigDialog(showConfigDialog); } @@ -2697,18 +2422,15 @@ void vpMbKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog) */ void vpMbKltMultiTracker::setOgreVisibilityTest(const bool &v) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setOgreVisibilityTest(v); } #ifdef VISP_HAVE_OGRE - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { - it->second->faces.getOgreContext()->setWindowName("Multi MBT Klt (" + - it->first + ")"); + it->second->faces.getOgreContext()->setWindowName("Multi MBT Klt (" + it->first + ")"); } #endif @@ -2721,11 +2443,9 @@ void vpMbKltMultiTracker::setOgreVisibilityTest(const bool &v) \param cameraName : Camera name to set the near clipping distance. \param dist : Near clipping value. */ -void vpMbKltMultiTracker::setNearClippingDistance( - const std::string &cameraName, const double &dist) +void vpMbKltMultiTracker::setNearClippingDistance(const std::string &cameraName, const double &dist) { - std::map::const_iterator it = - m_mapOfKltTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(cameraName); if (it != m_mapOfKltTrackers.end()) { it->second->setNearClippingDistance(dist); } else { @@ -2738,11 +2458,9 @@ void vpMbKltMultiTracker::setNearClippingDistance( \param opt : Optimization method to use. */ -void vpMbKltMultiTracker::setOptimizationMethod( - const vpMbtOptimizationMethod &opt) +void vpMbKltMultiTracker::setOptimizationMethod(const vpMbtOptimizationMethod &opt) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setOptimizationMethod(opt); } @@ -2757,12 +2475,10 @@ void vpMbKltMultiTracker::setOptimizationMethod( \param I : image corresponding to the desired pose. \param cMo_ : Pose to affect. */ -void vpMbKltMultiTracker::setPose(const vpImage &I, - const vpHomogeneousMatrix &cMo_) +void vpMbKltMultiTracker::setPose(const vpImage &I, const vpHomogeneousMatrix &cMo_) { if (m_mapOfKltTrackers.size() == 1) { - std::map::iterator it = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::iterator it = m_mapOfKltTrackers.find(m_referenceCameraName); if (it != m_mapOfKltTrackers.end()) { it->second->setPose(I, cMo_); this->cMo = cMo_; @@ -2771,8 +2487,7 @@ void vpMbKltMultiTracker::setPose(const vpImage &I, ctTc0.eye(); } else { std::stringstream ss; - ss << "Cannot find the reference camera: " << m_referenceCameraName - << " !"; + ss << "Cannot find the reference camera: " << m_referenceCameraName << " !"; throw vpException(vpTrackingException::fatalError, ss.str()); } } else { @@ -2794,15 +2509,12 @@ void vpMbKltMultiTracker::setPose(const vpImage &I, \param firstCameraIsReference : if true, the first camera is the reference, otherwise it is the second one. */ -void vpMbKltMultiTracker::setPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, +void vpMbKltMultiTracker::setPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference) { if (m_mapOfKltTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it = m_mapOfKltTrackers.begin(); it->second->setPose(I1, c1Mo); ++it; @@ -2819,8 +2531,7 @@ void vpMbKltMultiTracker::setPose(const vpImage &I1, ctTc0.eye(); } else { std::stringstream ss; - ss << "This method requires 2 cameras but there are " - << m_mapOfKltTrackers.size() << " cameras !"; + ss << "This method requires 2 cameras but there are " << m_mapOfKltTrackers.size() << " cameras !"; throw vpException(vpTrackingException::fatalError, ss.str()); } } @@ -2833,15 +2544,13 @@ void vpMbKltMultiTracker::setPose(const vpImage &I1, \param mapOfImages : Map of images. \param cMo_ : Pose to affect to the reference camera. */ -void vpMbKltMultiTracker::setPose( - const std::map *> &mapOfImages, - const vpHomogeneousMatrix &cMo_) +void vpMbKltMultiTracker::setPose(const std::map *> &mapOfImages, + const vpHomogeneousMatrix &cMo_) { - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); if (it_klt != m_mapOfKltTrackers.end()) { - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); if (it_img != mapOfImages.end()) { // Set pose on reference camera @@ -2854,35 +2563,28 @@ void vpMbKltMultiTracker::setPose( ctTc0.eye(); // Set the pose for the others cameras - for (it_klt = m_mapOfKltTrackers.begin(); - it_klt != m_mapOfKltTrackers.end(); ++it_klt) { + for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { if (it_klt->first != m_referenceCameraName) { - std::map::const_iterator - it_camTrans = - m_mapOfCameraTransformationMatrix.find(it_klt->first); + std::map::const_iterator it_camTrans = + m_mapOfCameraTransformationMatrix.find(it_klt->first); it_img = mapOfImages.find(it_klt->first); - if (it_camTrans != m_mapOfCameraTransformationMatrix.end() && - it_img != mapOfImages.end()) { + if (it_camTrans != m_mapOfCameraTransformationMatrix.end() && it_img != mapOfImages.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; it_klt->second->setPose(*it_img->second, cCurrentMo); } else { - throw vpException( - vpTrackingException::fatalError, - "Cannot find camera transformation matrix or image !"); + throw vpException(vpTrackingException::fatalError, "Cannot find camera transformation matrix or image !"); } } } } else { std::stringstream ss; - ss << "Missing image for reference camera: " << m_referenceCameraName - << " !"; + ss << "Missing image for reference camera: " << m_referenceCameraName << " !"; throw vpException(vpTrackingException::fatalError, ss.str().c_str()); } } else { std::stringstream ss; - ss << "The reference camera: " << m_referenceCameraName - << " does not exist !"; + ss << "The reference camera: " << m_referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::fatalError, ss.str().c_str()); } } @@ -2897,42 +2599,35 @@ void vpMbKltMultiTracker::setPose( \param mapOfImages : Map of images corresponding to the desired pose. \param mapOfCameraPoses : Map of poses to affect. */ -void vpMbKltMultiTracker::setPose( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses) +void vpMbKltMultiTracker::setPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses) { // Set the reference cMo - std::map::const_iterator it_klt = - m_mapOfKltTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(m_referenceCameraName); - - if (it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && - it_camPose != mapOfCameraPoses.end()) { + std::map::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName); + + if (it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) { it_klt->second->setPose(*it_img->second, it_camPose->second); it_klt->second->getPose(cMo); c0Mo = this->cMo; ctTc0.eye(); } else { - throw vpException(vpTrackingException::fatalError, - "Cannot set pose for the reference camera !"); + throw vpException(vpTrackingException::fatalError, "Cannot set pose for the reference camera !"); } // Vector of missing pose matrices for cameras std::vector vectorOfMissingCameraPoses; // Set pose for the specified cameras - for (it_klt = m_mapOfKltTrackers.begin(); - it_klt != m_mapOfKltTrackers.end(); ++it_klt) { + for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { if (it_klt->first != m_referenceCameraName) { it_img = mapOfImages.find(it_klt->first); it_camPose = mapOfCameraPoses.find(it_klt->first); - if (it_img != mapOfImages.end() && - it_camPose != mapOfCameraPoses.end()) { + if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) { // Set pose it_klt->second->setPose(*it_img->second, it_camPose->second); } else { @@ -2941,15 +2636,13 @@ void vpMbKltMultiTracker::setPose( } } - for (std::vector::const_iterator it1 = - vectorOfMissingCameraPoses.begin(); + for (std::vector::const_iterator it1 = vectorOfMissingCameraPoses.begin(); it1 != vectorOfMissingCameraPoses.end(); ++it1) { it_img = mapOfImages.find(*it1); std::map::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it1); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; m_mapOfKltTrackers[*it1]->setPose(*it_img->second, cCurrentMo); } else { @@ -2967,17 +2660,14 @@ void vpMbKltMultiTracker::setPose( \param referenceCameraName : Name of the reference camera. */ -void vpMbKltMultiTracker::setReferenceCameraName( - const std::string &referenceCameraName) +void vpMbKltMultiTracker::setReferenceCameraName(const std::string &referenceCameraName) { - std::map::const_iterator it = - m_mapOfKltTrackers.find(referenceCameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(referenceCameraName); if (it != m_mapOfKltTrackers.end()) { m_referenceCameraName = referenceCameraName; } else { std::stringstream ss; - ss << "The reference camera: " << referenceCameraName - << " does not exist !"; + ss << "The reference camera: " << referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::fatalError, ss.str().c_str()); } } @@ -2992,8 +2682,7 @@ void vpMbKltMultiTracker::setScanLineVisibilityTest(const bool &v) // Set general setScanLineVisibilityTest vpMbTracker::setScanLineVisibilityTest(v); - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setScanLineVisibilityTest(v); } @@ -3006,8 +2695,7 @@ void vpMbKltMultiTracker::setScanLineVisibilityTest(const bool &v) */ void vpMbKltMultiTracker::setKltThresholdAcceptation(const double th) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setKltThresholdAcceptation(th); } @@ -3022,11 +2710,9 @@ void vpMbKltMultiTracker::setKltThresholdAcceptation(const double th) \param name : name of the polygon(s). \param useKltTracking : True if it has to be considered, False otherwise. */ -void vpMbKltMultiTracker::setUseKltTracking(const std::string &name, - const bool &useKltTracking) +void vpMbKltMultiTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking) { - for (std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it = m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) { it->second->setUseKltTracking(name, useKltTracking); } @@ -3043,16 +2729,14 @@ void vpMbKltMultiTracker::track(const vpImage &I) { // Track only with reference camera // Get the reference camera parameters - std::map::const_iterator it = - m_mapOfKltTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName); if (it != m_mapOfKltTrackers.end()) { it->second->track(I); it->second->getPose(cMo); } else { std::stringstream ss; - ss << "The reference camera: " << m_referenceCameraName - << " does not exist !"; + ss << "The reference camera: " << m_referenceCameraName << " does not exist !"; throw vpException(vpTrackingException::fatalError, ss.str().c_str()); } } @@ -3065,12 +2749,10 @@ void vpMbKltMultiTracker::track(const vpImage &I) \param I1 : The first image. \param I2 : The second image. */ -void vpMbKltMultiTracker::track(const vpImage &I1, - const vpImage &I2) +void vpMbKltMultiTracker::track(const vpImage &I1, const vpImage &I2) { if (m_mapOfKltTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfKltTrackers.begin(); + std::map::const_iterator it = m_mapOfKltTrackers.begin(); std::map *> mapOfImages; mapOfImages[it->first] = &I1; ++it; @@ -3079,8 +2761,7 @@ void vpMbKltMultiTracker::track(const vpImage &I1, track(mapOfImages); } else { std::stringstream ss; - ss << "Require two cameras ! There are " << m_mapOfKltTrackers.size() - << " cameras !"; + ss << "Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !"; throw vpException(vpTrackingException::fatalError, ss.str().c_str()); } } @@ -3092,15 +2773,12 @@ void vpMbKltMultiTracker::track(const vpImage &I1, \param mapOfImages : Map of images. */ -void vpMbKltMultiTracker::track( - std::map *> &mapOfImages) +void vpMbKltMultiTracker::track(std::map *> &mapOfImages) { // Check if there is an image for each camera - for (std::map::const_iterator it_klt = - m_mapOfKltTrackers.begin(); + for (std::map::const_iterator it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) { - std::map *>::const_iterator - it_img = mapOfImages.find(it_klt->first); + std::map *>::const_iterator it_img = mapOfImages.find(it_klt->first); if (it_img == mapOfImages.end()) { throw vpException(vpTrackingException::fatalError, "Missing images !"); @@ -3110,8 +2788,7 @@ void vpMbKltMultiTracker::track( preTracking(mapOfImages); if (m_nbInfos < 4 || m_nbFaceUsed == 0) { - throw vpTrackingException(vpTrackingException::notEnoughPointError, - "Error: not enough features"); + throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features"); } computeVVS(); @@ -3126,10 +2803,7 @@ void vpMbKltMultiTracker::track( \return The number of features */ -std::map vpMbKltMultiTracker::getNbKltPoints() const -{ - return getKltNbPoints(); -} +std::map vpMbKltMultiTracker::getNbKltPoints() const { return getKltNbPoints(); } /*! Set the erosion of the mask used on the Model faces. @@ -3137,20 +2811,14 @@ std::map vpMbKltMultiTracker::getNbKltPoints() const \param e : The desired erosion. */ -void vpMbKltMultiTracker::setMaskBorder(const unsigned int &e) -{ - setKltMaskBorder(e); -} +void vpMbKltMultiTracker::setMaskBorder(const unsigned int &e) { setKltMaskBorder(e); } /*! Set the threshold for the acceptation of a point. \param th : Threshold for the weight below which a point is rejected. */ -void vpMbKltMultiTracker::setThresholdAcceptation(const double th) -{ - setKltThresholdAcceptation(th); -} +void vpMbKltMultiTracker::setThresholdAcceptation(const double th) { setKltThresholdAcceptation(th); } #elif !defined(VISP_BUILD_SHARED_LIBS) // Work arround to avoid warning: diff --git a/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp b/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp index e4db0114a5..20af8e28f4 100644 --- a/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp +++ b/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp @@ -42,11 +42,10 @@ #include #include -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin) -#include // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro +#include // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro #endif vpMbKltTracker::vpMbKltTracker() @@ -56,10 +55,9 @@ vpMbKltTracker::vpMbKltTracker() #else cur(NULL), #endif - c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), - percentGood(0.6), ctTc0(), tracker(), kltPolygons(), kltCylinders(), - circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), - m_w_klt(), m_weightedError_klt(), m_robust_klt() + c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), percentGood(0.6), ctTc0(), tracker(), + kltPolygons(), kltCylinders(), circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), m_w_klt(), + m_weightedError_klt(), m_robust_klt() { tracker.setTrackerId(1); tracker.setUseHarris(1); @@ -96,9 +94,7 @@ vpMbKltTracker::~vpMbKltTracker() #endif // delete the Klt Polygon features - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; if (kltpoly != NULL) { delete kltpoly; @@ -107,9 +103,8 @@ vpMbKltTracker::~vpMbKltTracker() } kltPolygons.clear(); - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; if (kltPolyCylinder != NULL) { delete kltPolyCylinder; @@ -119,9 +114,7 @@ vpMbKltTracker::~vpMbKltTracker() kltCylinders.clear(); // delete the structures used to display circles - for (std::list::const_iterator it = - circles_disp.begin(); - it != circles_disp.end(); ++it) { + for (std::list::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) { vpMbtDistanceCircle *ci = *it; if (ci != NULL) { delete ci; @@ -140,8 +133,7 @@ void vpMbKltTracker::init(const vpImage &I) bool reInitialisation = false; if (!useOgre) - faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); else { #ifdef VISP_HAVE_OGRE if (!faces.isOgreInitialised()) { @@ -154,12 +146,10 @@ void vpMbKltTracker::init(const vpImage &I) ogreShowConfigDialog = false; } - faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); #else - faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); #endif } reinit(I); @@ -183,8 +173,7 @@ void vpMbKltTracker::reinit(const vpImage &I) #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); + IplImage *mask = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1); cvZero(mask); #endif @@ -194,35 +183,26 @@ void vpMbKltTracker::reinit(const vpImage &I) vpImageConvert::convert(faces.getMbScanLineRenderer().getMask(), mask); } else { unsigned char val = 255 /* - i*15*/; - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { kltpoly = *it; - if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && - kltpoly->polygon->getNbPoint() > 2) { + if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) { // need to changeFrame when reinit() is called by postTracking // other solution is kltpoly->polygon->changeFrame(cMo); - kltpoly->polygon->computePolygonClipped( - cam); // Might not be necessary when scanline is activated + kltpoly->polygon->computePolygonClipped(cam); // Might not be necessary when scanline is activated kltpoly->updateMask(mask, val, maskBorder); } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { kltPolyCylinder = *it; if (kltPolyCylinder->isTracked()) { - for (unsigned int k = 0; - k < kltPolyCylinder->listIndicesCylinderBBox.size(); k++) { - unsigned int indCylBBox = - (unsigned int)kltPolyCylinder->listIndicesCylinderBBox[k]; - if (faces[indCylBBox]->isVisible() && - faces[indCylBBox]->getNbPoint() > 2u) { - faces[indCylBBox]->computePolygonClipped( - cam); // Might not be necessary when scanline is activated + for (unsigned int k = 0; k < kltPolyCylinder->listIndicesCylinderBBox.size(); k++) { + unsigned int indCylBBox = (unsigned int)kltPolyCylinder->listIndicesCylinderBBox[k]; + if (faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2u) { + faces[indCylBBox]->computePolygonClipped(cam); // Might not be necessary when scanline is activated } } @@ -237,19 +217,15 @@ void vpMbKltTracker::reinit(const vpImage &I) // vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << " // points" << std::endl; - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { kltpoly = *it; - if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && - kltpoly->polygon->getNbPoint() > 2) { + if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) { kltpoly->init(tracker); } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { kltPolyCylinder = *it; if (kltPolyCylinder->isTracked()) @@ -277,9 +253,7 @@ void vpMbKltTracker::resetTracker() #endif // delete the Klt Polygon features - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; if (kltpoly != NULL) { delete kltpoly; @@ -288,9 +262,8 @@ void vpMbKltTracker::resetTracker() } kltPolygons.clear(); - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; if (kltPolyCylinder != NULL) { delete kltPolyCylinder; @@ -300,9 +273,7 @@ void vpMbKltTracker::resetTracker() kltCylinders.clear(); // delete the structures used to display circles - for (std::list::const_iterator it = - circles_disp.begin(); - it != circles_disp.end(); ++it) { + for (std::list::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) { vpMbtDistanceCircle *ci = *it; if (ci != NULL) { delete ci; @@ -361,8 +332,7 @@ void vpMbKltTracker::resetTracker() std::vector vpMbKltTracker::getKltImagePoints() const { std::vector kltPoints; - for (unsigned int i = 0; - i < static_cast(tracker.getNbFeatures()); i++) { + for (unsigned int i = 0; i < static_cast(tracker.getNbFeatures()); i++) { long id; float x_tmp, y_tmp; tracker.getFeature((int)i, id, x_tmp, y_tmp); @@ -383,8 +353,7 @@ std::vector vpMbKltTracker::getKltImagePoints() const std::map vpMbKltTracker::getKltImagePointsWithId() const { std::map kltPoints; - for (unsigned int i = 0; - i < static_cast(tracker.getNbFeatures()); i++) { + for (unsigned int i = 0; i < static_cast(tracker.getNbFeatures()); i++) { long id; float x_tmp, y_tmp; tracker.getFeature((int)i, id, x_tmp, y_tmp); @@ -425,16 +394,13 @@ void vpMbKltTracker::setCameraParameters(const vpCameraParameters &camera) // faces[i]->setCameraParameters(camera); // } - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; kltpoly->setCameraParameters(camera); } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; kltPolyCylinder->setCameraParameters(camera); } @@ -451,15 +417,13 @@ void vpMbKltTracker::setCameraParameters(const vpCameraParameters &camera) \param I : image corresponding to the desired pose. \param cdMo : Pose to affect. */ -void vpMbKltTracker::setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo) +void vpMbKltTracker::setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo) { if (!kltCylinders.empty()) { 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; + std::cout << "Tracker will be reinitialized with the given pose." << std::endl; cMo = cdMo; init(I); } else { @@ -471,24 +435,19 @@ void vpMbKltTracker::setPose(const vpImage &I, std::vector guess_pts; #else unsigned int nbp = 0; - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::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)); + 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])); + guess_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(guess_pts[0])); #endif vpHomogeneousMatrix cdMc = cdMo * cMo.inverse(); @@ -502,18 +461,14 @@ void vpMbKltTracker::setPose(const vpImage &I, // unsigned int nbCur = 0; - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { kltpoly = *it; - if (kltpoly->polygon->isVisible() && - kltpoly->polygon->getNbPoint() > 2) { + 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]); + vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]); plan.changeFrame(cMcd); vpColVector Nc = plan.getNormal(); @@ -530,22 +485,16 @@ void vpMbKltTracker::setPose(const vpImage &I, vpMatrix cdGc = cam.get_K() * cdHc * cam.get_K_inverse(); // Points displacement - std::map::const_iterator iter = - kltpoly->getCurrentPoints().begin(); + std::map::const_iterator iter = kltpoly->getCurrentPoints().begin(); // nbCur+= (unsigned int)kltpoly->getCurrentPoints().size(); for (; iter != kltpoly->getCurrentPoints().end(); ++iter) { #if (VISP_HAVE_OPENCV_VERSION >= 0x020408) #if TARGET_OS_IPHONE - if (std::find( - init_ids.begin(), init_ids.end(), - (long)(kltpoly->getCurrentPointsInd())[(int)iter->first]) != + if (std::find(init_ids.begin(), init_ids.end(), (long)(kltpoly->getCurrentPointsInd())[(int)iter->first]) != init_ids.end()) #else - if (std::find( - init_ids.begin(), init_ids.end(), - (long)(kltpoly - ->getCurrentPointsInd())[(size_t)iter->first]) != - init_ids.end()) + if (std::find(init_ids.begin(), init_ids.end(), + (long)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]) != init_ids.end()) #endif { // KLT point already processed (a KLT point can exist in another @@ -563,33 +512,26 @@ void vpMbKltTracker::setPose(const vpImage &I, cv::Point2f p((float)cdp[0], (float)cdp[1]); init_pts.push_back(p); #if TARGET_OS_IPHONE - init_ids.push_back( - (size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]); + init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]); #else - init_ids.push_back( - (size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]); + init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]); #endif #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]; + 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]; + 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::epsilon()) { cdp[0] = 0.0; cdp[1] = 0.0; - throw vpException(vpException::divideByZeroError, - "the depth of the point is calculated to zero"); + 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; + 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) @@ -625,15 +567,12 @@ void vpMbKltTracker::setPose(const vpImage &I, bool reInitialisation = false; if (!useOgre) - faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation); else { #ifdef VISP_HAVE_OGRE - faces.setVisibleOgre(I, cam, cdMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisibleOgre(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation); #else - faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation); #endif } @@ -644,12 +583,9 @@ void vpMbKltTracker::setPose(const vpImage &I, faces.computeScanLineRender(cam, I.getWidth(), I.getHeight()); } - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { kltpoly = *it; - if (kltpoly->polygon->isVisible() && - kltpoly->polygon->getNbPoint() > 2) { + if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) { kltpoly->polygon->computePolygonClipped(cam); kltpoly->init(tracker); } @@ -706,12 +642,9 @@ void vpMbKltTracker::preTracking(const vpImage &I) m_nbInfos = 0; m_nbFaceUsed = 0; // for (unsigned int i = 0; i < faces.size(); i += 1){ - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; - if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && - kltpoly->polygon->getNbPoint() > 2) { + if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) { kltpoly->computeNbDetectedCurrent(tracker); // faces[i]->ransac(); if (kltpoly->hasEnoughPoints()) { @@ -721,9 +654,8 @@ void vpMbKltTracker::preTracking(const vpImage &I) } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; if (kltPolyCylinder->isTracked()) { @@ -739,8 +671,7 @@ void vpMbKltTracker::preTracking(const vpImage &I) /*! Realize the post tracking operations. Mostly visibility tests */ -bool vpMbKltTracker::postTracking(const vpImage &I, - vpColVector &w) +bool vpMbKltTracker::postTracking(const vpImage &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 @@ -751,12 +682,9 @@ bool vpMbKltTracker::postTracking(const vpImage &I, unsigned int currentNumber = 0; unsigned int shift = 0; // for (unsigned int i = 0; i < faces.size(); i += 1){ - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; - if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && - kltpoly->polygon->getNbPoint() > 2) { + if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) { initialNumber += kltpoly->getInitialNumberPoint(); if (kltpoly->hasEnoughPoints()) { vpSubColVector sub_w(w, shift, 2 * kltpoly->getCurrentNumberPoints()); @@ -772,16 +700,14 @@ bool vpMbKltTracker::postTracking(const vpImage &I, } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; if (kltPolyCylinder->isTracked()) { initialNumber += kltPolyCylinder->getInitialNumberPoint(); if (kltPolyCylinder->hasEnoughPoints()) { - vpSubColVector sub_w(w, shift, - 2 * kltPolyCylinder->getCurrentNumberPoints()); + vpSubColVector sub_w(w, shift, 2 * kltPolyCylinder->getCurrentNumberPoints()); shift += 2 * kltPolyCylinder->getCurrentNumberPoints(); kltPolyCylinder->removeOutliers(sub_w, threshold_outlier); @@ -798,15 +724,12 @@ bool vpMbKltTracker::postTracking(const vpImage &I, reInitialisation = true; } else { if (!useOgre) - faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); else { #ifdef VISP_HAVE_OGRE - faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); #else - faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); #endif } } @@ -844,8 +767,7 @@ void vpMbKltTracker::computeVVS() vpMbKltTracker::computeVVSInteractionMatrixAndResidu(); bool reStartFromLastIncrement = false; - computeVVSCheckLevenbergMarquardt(iter, m_error_klt, error_prev, cMoPrev, - mu, reStartFromLastIncrement); + computeVVSCheckLevenbergMarquardt(iter, m_error_klt, error_prev, cMoPrev, mu, reStartFromLastIncrement); if (reStartFromLastIncrement) { ctTc0 = ctTc0_Prev; } @@ -878,9 +800,8 @@ void vpMbKltTracker::computeVVS() } } - computeVVSPoseEstimation(isoJoIdentity, iter, m_L_klt, LTL, - m_weightedError_klt, m_error_klt, error_prev, - LTR, mu, v); + computeVVSPoseEstimation(isoJoIdentity, iter, m_L_klt, LTL, m_weightedError_klt, m_error_klt, error_prev, LTR, mu, + v); cMoPrev = cMo; ctTc0_Prev = ctTc0; @@ -891,8 +812,7 @@ void vpMbKltTracker::computeVVS() iter++; } - computeCovarianceMatrixVVS(isoJoIdentity, m_w_klt, cMoPrev, L_true, - LVJ_true, m_error_klt); + computeCovarianceMatrixVVS(isoJoIdentity, m_w_klt, cMoPrev, L_true, LVJ_true, m_error_klt); } void vpMbKltTracker::computeVVSInit() @@ -915,45 +835,36 @@ void vpMbKltTracker::computeVVSInteractionMatrixAndResidu() unsigned int shift = 0; vpHomography H; - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; - if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && - kltpoly->polygon->getNbPoint() > 2 && kltpoly->hasEnoughPoints()) { - vpSubColVector subR(m_error_klt, shift, - 2 * kltpoly->getCurrentNumberPoints()); - vpSubMatrix subL(m_L_klt, shift, 0, - 2 * kltpoly->getCurrentNumberPoints(), 6); + if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 && + kltpoly->hasEnoughPoints()) { + vpSubColVector subR(m_error_klt, shift, 2 * kltpoly->getCurrentNumberPoints()); + vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6); try { kltpoly->computeHomography(ctTc0, H); kltpoly->computeInteractionMatrixAndResidu(subR, subL); } catch (...) { - throw vpTrackingException(vpTrackingException::fatalError, - "Cannot compute interaction matrix"); + throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix"); } shift += 2 * kltpoly->getCurrentNumberPoints(); } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) { - vpSubColVector subR(m_error_klt, shift, - 2 * kltPolyCylinder->getCurrentNumberPoints()); - vpSubMatrix subL(m_L_klt, shift, 0, - 2 * kltPolyCylinder->getCurrentNumberPoints(), 6); + vpSubColVector subR(m_error_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints()); + vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6); try { kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL); } catch (...) { - throw vpTrackingException(vpTrackingException::fatalError, - "Cannot compute interaction matrix"); + throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix"); } shift += 2 * kltPolyCylinder->getCurrentNumberPoints(); @@ -973,8 +884,7 @@ void vpMbKltTracker::track(const vpImage &I) preTracking(I); if (m_nbInfos < 4 || m_nbFaceUsed == 0) { - throw vpTrackingException(vpTrackingException::notEnoughPointError, - "Error: not enough features"); + throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features"); } computeVVS(); @@ -1064,13 +974,11 @@ void vpMbKltTracker::loadConfigFile(const char *configFile) xmlp.setAngleDisappear(vpMath::deg(angleDisappears)); try { - std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " - << std::endl; + 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."); + throw vpException(vpException::ioError, "problem to parse configuration file."); } vpCameraParameters camera; @@ -1128,10 +1036,8 @@ void vpMbKltTracker::loadConfigFile(const char *configFile) \param displayFullModel : Boolean to say if all the model has to be displayed, even the faces that are visible. */ -void vpMbKltTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &camera, - const vpColor &col, const unsigned int thickness, +void vpMbKltTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { vpCameraParameters c = camera; @@ -1154,36 +1060,28 @@ void vpMbKltTracker::display(const vpImage &I, if (useScanLine && !displayFullModel) faces.computeScanLineRender(cam, I.getWidth(), I.getHeight()); - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; kltpoly->display(I, cMo_, camera, col, thickness, displayFullModel); - if (displayFeatures && kltpoly->hasEnoughPoints() && - kltpoly->polygon->isVisible() && kltpoly->isTracked()) { + if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) { kltpoly->displayPrimitive(I); // faces[i]->displayNormal(I); } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; - kltPolyCylinder->display(I, cMo_, camera, col, thickness, - displayFullModel); + kltPolyCylinder->display(I, cMo_, camera, col, thickness, displayFullModel); - if (displayFeatures && kltPolyCylinder->isTracked() && - kltPolyCylinder->hasEnoughPoints()) + if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) kltPolyCylinder->displayPrimitive(I); } - for (std::list::const_iterator it = - circles_disp.begin(); - it != circles_disp.end(); ++it) { + for (std::list::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } @@ -1204,10 +1102,8 @@ void vpMbKltTracker::display(const vpImage &I, \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 &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &camera, - const vpColor &col, const unsigned int thickness, +void vpMbKltTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { vpCameraParameters c = camera; @@ -1230,36 +1126,28 @@ void vpMbKltTracker::display(const vpImage &I, if (useScanLine && !displayFullModel) faces.computeScanLineRender(cam, I.getWidth(), I.getHeight()); - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; kltpoly->display(I, cMo_, camera, col, thickness, displayFullModel); - if (displayFeatures && kltpoly->hasEnoughPoints() && - kltpoly->polygon->isVisible() && kltpoly->isTracked()) { + if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) { kltpoly->displayPrimitive(I); // faces[i]->displayNormal(I); } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; - kltPolyCylinder->display(I, cMo_, camera, col, thickness, - displayFullModel); + kltPolyCylinder->display(I, cMo_, camera, col, thickness, displayFullModel); - if (displayFeatures && kltPolyCylinder->isTracked() && - kltPolyCylinder->hasEnoughPoints()) + if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) kltPolyCylinder->displayPrimitive(I); } - for (std::list::const_iterator it = - circles_disp.begin(); - it != circles_disp.end(); ++it) { + for (std::list::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness); } @@ -1281,31 +1169,25 @@ void vpMbKltTracker::testTracking() { unsigned int nbTotalPoints = 0; // for (unsigned int i = 0; i < faces.size(); i += 1){ - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; - if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && - kltpoly->polygon->getNbPoint() > 2 && kltpoly->hasEnoughPoints()) { + if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 && + kltpoly->hasEnoughPoints()) { nbTotalPoints += kltpoly->getCurrentNumberPoints(); } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *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)."); + 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)."); } } @@ -1319,8 +1201,7 @@ void vpMbKltTracker::testTracking() \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, +void vpMbKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace, const std::string & /*name*/) { vpMbtDistanceKltCylinder *kltPoly = new vpMbtDistanceKltCylinder(); @@ -1350,8 +1231,7 @@ void vpMbKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, 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, +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); @@ -1365,8 +1245,7 @@ void vpMbKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, 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, +void vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name) { bool already_here = false; @@ -1403,10 +1282,8 @@ void vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, 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 &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbKltTracker::reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose) { reInitModel(I, cad_name.c_str(), cMo_, verbose); } @@ -1420,9 +1297,7 @@ void vpMbKltTracker::reInitModel(const vpImage &I, 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 &I, - const char *cad_name, - const vpHomogeneousMatrix &cMo_, +void vpMbKltTracker::reInitModel(const vpImage &I, const char *cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose) { this->cMo.eye(); @@ -1437,9 +1312,7 @@ void vpMbKltTracker::reInitModel(const vpImage &I, firstInitialisation = true; // delete the Klt Polygon features - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; if (kltpoly != NULL) { delete kltpoly; @@ -1448,9 +1321,8 @@ void vpMbKltTracker::reInitModel(const vpImage &I, } kltPolygons.clear(); - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; if (kltPolyCylinder != NULL) { delete kltPolyCylinder; @@ -1460,9 +1332,7 @@ void vpMbKltTracker::reInitModel(const vpImage &I, kltCylinders.clear(); // delete the structures used to display circles - for (std::list::const_iterator it = - circles_disp.begin(); - it != circles_disp.end(); ++it) { + for (std::list::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) { vpMbtDistanceCircle *ci = *it; if (ci != NULL) { delete ci; @@ -1483,12 +1353,9 @@ void vpMbKltTracker::reInitModel(const vpImage &I, \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) +void vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking) { - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; if (kltpoly->polygon->getName() == name) { kltpoly->setTracked(useKltTracking); diff --git a/modules/tracker/mbt/src/klt/vpMbtDistanceKltCylinder.cpp b/modules/tracker/mbt/src/klt/vpMbtDistanceKltCylinder.cpp index 8824a30f9f..4db4d37254 100644 --- a/modules/tracker/mbt/src/klt/vpMbtDistanceKltCylinder.cpp +++ b/modules/tracker/mbt/src/klt/vpMbtDistanceKltCylinder.cpp @@ -40,15 +40,14 @@ #include #include -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) #if defined(VISP_HAVE_CLIPPER) #include // clipper private library #endif #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin) -#include // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro +#include // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro #endif /*! @@ -56,11 +55,9 @@ */ 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) + : 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) { } @@ -70,8 +67,7 @@ vpMbtDistanceKltCylinder::vpMbtDistanceKltCylinder() */ vpMbtDistanceKltCylinder::~vpMbtDistanceKltCylinder() {} -void vpMbtDistanceKltCylinder::buildFrom(const vpPoint &p1, const vpPoint &p2, - const double &r) +void vpMbtDistanceKltCylinder::buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r) { p1Ext = p1; p2Ext = p2; @@ -91,16 +87,12 @@ void vpMbtDistanceKltCylinder::buildFrom(const vpPoint &p1, const vpPoint &p2, 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); + 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); + 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); } /*! @@ -111,8 +103,7 @@ void vpMbtDistanceKltCylinder::buildFrom(const vpPoint &p1, const vpPoint &p2, \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) +void vpMbtDistanceKltCylinder::init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo) { c0Mo = cMo; cylinder.changeFrame(cMo); @@ -125,8 +116,7 @@ void vpMbtDistanceKltCylinder::init(const vpKltOpencv &_tracker, curPoints = std::map(); curPointsInd = std::map(); - for (unsigned int i = 0; - i < static_cast(_tracker.getNbFeatures()); i++) { + for (unsigned int i = 0; i < static_cast(_tracker.getNbFeatures()); i++) { long id; float x_tmp, y_tmp; _tracker.getFeature((int)i, id, x_tmp, y_tmp); @@ -134,15 +124,10 @@ void vpMbtDistanceKltCylinder::init(const vpKltOpencv &_tracker, bool add = false; if (useScanLine) { - if ((unsigned int)y_tmp < hiddenface->getMbScanLineRenderer() - .getPrimitiveIDs() - .getHeight() && - (unsigned int)x_tmp < hiddenface->getMbScanLineRenderer() - .getPrimitiveIDs() - .getWidth()) { + 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] == + if (hiddenface->getMbScanLineRenderer().getPrimitiveIDs()[(unsigned int)y_tmp][(unsigned int)x_tmp] == listIndicesCylinderBBox[kc]) { add = true; break; @@ -151,8 +136,7 @@ void vpMbtDistanceKltCylinder::init(const vpKltOpencv &_tracker, } else { std::vector roi; for (unsigned int kc = 0; kc < listIndicesCylinderBBox.size(); kc++) { - hiddenface->getPolygon()[(size_t)listIndicesCylinderBBox[kc]] - ->getRoiClipped(cam, roi); + hiddenface->getPolygon()[(size_t)listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi); if (vpPolygon::isInside(roi, y_tmp, x_tmp)) { add = true; break; @@ -209,8 +193,7 @@ void vpMbtDistanceKltCylinder::init(const vpKltOpencv &_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) +unsigned int vpMbtDistanceKltCylinder::computeNbDetectedCurrent(const vpKltOpencv &_tracker) { long id; float x, y; @@ -218,17 +201,14 @@ unsigned int vpMbtDistanceKltCylinder::computeNbDetectedCurrent( curPoints = std::map(); curPointsInd = std::map(); - for (unsigned int i = 0; - i < static_cast(_tracker.getNbFeatures()); i++) { + for (unsigned int i = 0; i < static_cast(_tracker.getNbFeatures()); i++) { _tracker.getFeature((int)i, id, x, y); if (isTrackedFeature((int)id)) { #if TARGET_OS_IPHONE - curPoints[(int)id] = - vpImagePoint(static_cast(y), static_cast(x)); + curPoints[(int)id] = vpImagePoint(static_cast(y), static_cast(x)); curPointsInd[(int)id] = (int)i; #else - curPoints[id] = - vpImagePoint(static_cast(y), static_cast(x)); + curPoints[id] = vpImagePoint(static_cast(y), static_cast(x)); curPointsInd[id] = (int)i; #endif nbPointsCur++; @@ -251,8 +231,7 @@ unsigned int vpMbtDistanceKltCylinder::computeNbDetectedCurrent( \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) +void vpMbtDistanceKltCylinder::removeOutliers(const vpColVector &_w, const double &threshold_outlier) { std::map tmp; std::map tmp2; @@ -264,8 +243,7 @@ void vpMbtDistanceKltCylinder::removeOutliers(const vpColVector &_w, 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()); + tmp[iter->first] = vpImagePoint(iter->second.get_i(), iter->second.get_j()); tmp2[iter->first] = curPointsInd[iter->first]; nbPointsCur++; } else { @@ -300,8 +278,8 @@ void vpMbtDistanceKltCylinder::removeOutliers(const vpColVector &_w, \param _R : the residu vector \param _J : the interaction matrix */ -void vpMbtDistanceKltCylinder::computeInteractionMatrixAndResidu( - const vpHomogeneousMatrix &_cMc0, vpColVector &_R, vpMatrix &_J) +void vpMbtDistanceKltCylinder::computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &_cMc0, vpColVector &_R, + vpMatrix &_J) { unsigned int index_ = 0; @@ -412,24 +390,19 @@ void vpMbtDistanceKltCylinder::updateMask( #endif for (unsigned int kc = 0; kc < listIndicesCylinderBBox.size(); kc++) { - if ((*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]] - ->isVisible() && - (*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]] - ->getNbPoint() > 2) { + if ((*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]]->isVisible() && + (*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]]->getNbPoint() > 2) { int i_min, i_max, j_min, j_max; std::vector roi; - (*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]]->getRoiClipped( - cam, roi); + (*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi); double shiftBorder_d = (double)shiftBorder; #if defined(VISP_HAVE_CLIPPER) std::vector roi_offset; ClipperLib::Path path; - for (std::vector::const_iterator it = roi.begin(); - it != roi.end(); ++it) { - path.push_back(ClipperLib::IntPoint((ClipperLib::cInt)it->get_u(), - (ClipperLib::cInt)it->get_v())); + for (std::vector::const_iterator it = roi.begin(); it != roi.end(); ++it) { + path.push_back(ClipperLib::IntPoint((ClipperLib::cInt)it->get_u(), (ClipperLib::cInt)it->get_v())); } ClipperLib::Paths solution; @@ -449,8 +422,7 @@ void vpMbtDistanceKltCylinder::updateMask( std::vector corners; for (size_t j = 0; j < solution[i].size(); j++) { - corners.push_back(vpImagePoint((double)(solution[i][j].Y), - (double)(solution[i][j].X))); + corners.push_back(vpImagePoint((double)(solution[i][j].Y), (double)(solution[i][j].X))); } polygon_area.buildFrom(corners); @@ -462,9 +434,7 @@ void vpMbtDistanceKltCylinder::updateMask( } for (size_t i = 0; i < solution[index_max].size(); i++) { - roi_offset.push_back( - vpImagePoint((double)(solution[index_max][i].Y), - (double)(solution[index_max][i].X))); + roi_offset.push_back(vpImagePoint((double)(solution[index_max][i].Y), (double)(solution[index_max][i].X))); } } else { roi_offset = roi; @@ -509,14 +479,10 @@ void vpMbtDistanceKltCylinder::updateMask( #else 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)) { + 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(i, j) = nb; } } else { @@ -528,22 +494,17 @@ void vpMbtDistanceKltCylinder::updateMask( } } #else - unsigned char *ptrData = - (unsigned char *)mask->imageData + i_min * mask->widthStep + j_min; + 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)) { + 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++; @@ -568,8 +529,7 @@ void vpMbtDistanceKltCylinder::updateMask( \param _I : The image where to display. */ -void vpMbtDistanceKltCylinder::displayPrimitive( - const vpImage &_I) +void vpMbtDistanceKltCylinder::displayPrimitive(const vpImage &_I) { std::map::const_iterator iter = curPoints.begin(); for (; iter != curPoints.end(); ++iter) { @@ -612,12 +572,9 @@ void vpMbtDistanceKltCylinder::displayPrimitive(const vpImage &_I) } } -void vpMbtDistanceKltCylinder::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, - const bool /*displayFullModel*/) +void vpMbtDistanceKltCylinder::display(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &camera, const vpColor &col, + const unsigned int thickness, const bool /*displayFullModel*/) { // if(isvisible || displayFullModel) { @@ -643,10 +600,8 @@ void vpMbtDistanceKltCylinder::display(const vpImage &I, 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); + 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; @@ -670,12 +625,9 @@ void vpMbtDistanceKltCylinder::display(const vpImage &I, } } -void vpMbtDistanceKltCylinder::display(const vpImage &I, - const vpHomogeneousMatrix &cMo, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, - const bool /*displayFullModel*/) +void vpMbtDistanceKltCylinder::display(const vpImage &I, const vpHomogeneousMatrix &cMo, + const vpCameraParameters &camera, const vpColor &col, + const unsigned int thickness, const bool /*displayFullModel*/) { // if(isvisible || displayFullModel) { @@ -701,23 +653,17 @@ void vpMbtDistanceKltCylinder::display(const vpImage &I, 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); + 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, 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); + 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; diff --git a/modules/tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp b/modules/tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp index 497b0889e9..8a2e57e5c2 100644 --- a/modules/tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp +++ b/modules/tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp @@ -40,15 +40,14 @@ #include #include -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) #if defined(VISP_HAVE_CLIPPER) #include // clipper private library #endif #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin) -#include // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro +#include // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro #endif /*! @@ -56,13 +55,10 @@ */ vpMbtDistanceKltPoints::vpMbtDistanceKltPoints() - : H(), N(), N_cur(), invd0(1.), cRc0_0n(), - initPoints(std::map()), - curPoints(std::map()), - curPointsInd(std::map()), nbPointsCur(0), nbPointsInit(0), - minNbPoint(4), enoughPoints(false), dt(1.), d0(1.), cam(), - isTrackedKltPoints(true), polygon(NULL), hiddenface(NULL), - useScanLine(false) + : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(std::map()), + curPoints(std::map()), curPointsInd(std::map()), nbPointsCur(0), nbPointsInit(0), + minNbPoint(4), enoughPoints(false), dt(1.), d0(1.), cam(), isTrackedKltPoints(true), polygon(NULL), + hiddenface(NULL), useScanLine(false) { } @@ -90,8 +86,7 @@ void vpMbtDistanceKltPoints::init(const vpKltOpencv &_tracker) std::vector roi; polygon->getRoiClipped(cam, roi); - for (unsigned int i = 0; - i < static_cast(_tracker.getNbFeatures()); i++) { + for (unsigned int i = 0; i < static_cast(_tracker.getNbFeatures()); i++) { long id; float x_tmp, y_tmp; _tracker.getFeature((int)i, id, x_tmp, y_tmp); @@ -99,14 +94,10 @@ void vpMbtDistanceKltPoints::init(const vpKltOpencv &_tracker) 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()) + 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; @@ -134,8 +125,7 @@ void vpMbtDistanceKltPoints::init(const vpKltOpencv &_tracker) enoughPoints = false; // initialisation of the value for the computation in SE3 - vpPlane plan(polygon->getPoint(0), polygon->getPoint(1), - polygon->getPoint(2)); + vpPlane plan(polygon->getPoint(0), polygon->getPoint(1), polygon->getPoint(2)); d0 = plan.getD(); N = plan.getNormal(); @@ -153,8 +143,7 @@ void vpMbtDistanceKltPoints::init(const vpKltOpencv &_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) +unsigned int vpMbtDistanceKltPoints::computeNbDetectedCurrent(const vpKltOpencv &_tracker) { long id; float x, y; @@ -162,17 +151,14 @@ vpMbtDistanceKltPoints::computeNbDetectedCurrent(const vpKltOpencv &_tracker) curPoints = std::map(); curPointsInd = std::map(); - for (unsigned int i = 0; - i < static_cast(_tracker.getNbFeatures()); i++) { + for (unsigned int i = 0; i < static_cast(_tracker.getNbFeatures()); i++) { _tracker.getFeature((int)i, id, x, y); if (isTrackedFeature((int)id)) { #if TARGET_OS_IPHONE - curPoints[(int)id] = - vpImagePoint(static_cast(y), static_cast(x)); + curPoints[(int)id] = vpImagePoint(static_cast(y), static_cast(x)); curPointsInd[(int)id] = (int)i; #else - curPoints[id] = - vpImagePoint(static_cast(y), static_cast(x)); + curPoints[id] = vpImagePoint(static_cast(y), static_cast(x)); curPointsInd[id] = (int)i; #endif } @@ -198,8 +184,7 @@ vpMbtDistanceKltPoints::computeNbDetectedCurrent(const vpKltOpencv &_tracker) \param _R : the residu vector \param _J : the interaction matrix */ -void vpMbtDistanceKltPoints::computeInteractionMatrixAndResidu( - vpColVector &_R, vpMatrix &_J) +void vpMbtDistanceKltPoints::computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J) { unsigned int index_ = 0; @@ -241,8 +226,7 @@ void vpMbtDistanceKltPoints::computeInteractionMatrixAndResidu( } } -double vpMbtDistanceKltPoints::compute_1_over_Z(const double x, - const double y) +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); @@ -261,10 +245,7 @@ double vpMbtDistanceKltPoints::compute_1_over_Z(const double x, \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, +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]; @@ -272,8 +253,7 @@ inline void vpMbtDistanceKltPoints::computeP_mu_t(const double x_in, if (fabs(p_mu_t_2) < std::numeric_limits::epsilon()) { x_out = 0.0; y_out = 0.0; - throw vpException(vpException::divideByZeroError, - "the depth of the point is calculated to zero"); + 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; @@ -293,8 +273,7 @@ inline void vpMbtDistanceKltPoints::computeP_mu_t(const double x_in, position of the camera and the current camera position \param _cHc0 : the homography of the plane */ -void vpMbtDistanceKltPoints::computeHomography( - const vpHomogeneousMatrix &_cTc0, vpHomography &_cHc0) +void vpMbtDistanceKltPoints::computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &_cHc0) { vpRotationMatrix cRc0; vpTranslationVector ctransc0; @@ -391,10 +370,8 @@ void vpMbtDistanceKltPoints::updateMask( std::vector roi_offset; ClipperLib::Path path; - for (std::vector::const_iterator it = roi.begin(); - it != roi.end(); ++it) { - path.push_back(ClipperLib::IntPoint((ClipperLib::cInt)it->get_u(), - (ClipperLib::cInt)it->get_v())); + for (std::vector::const_iterator it = roi.begin(); it != roi.end(); ++it) { + path.push_back(ClipperLib::IntPoint((ClipperLib::cInt)it->get_u(), (ClipperLib::cInt)it->get_v())); } ClipperLib::Paths solution; @@ -414,8 +391,7 @@ void vpMbtDistanceKltPoints::updateMask( std::vector corners; for (size_t j = 0; j < solution[i].size(); j++) { - corners.push_back(vpImagePoint((double)(solution[i][j].Y), - (double)(solution[i][j].X))); + corners.push_back(vpImagePoint((double)(solution[i][j].Y), (double)(solution[i][j].X))); } polygon_area.buildFrom(corners); @@ -427,8 +403,7 @@ void vpMbtDistanceKltPoints::updateMask( } for (size_t i = 0; i < solution[index_max].size(); i++) { - roi_offset.push_back(vpImagePoint((double)(solution[index_max][i].Y), - (double)(solution[index_max][i].X))); + roi_offset.push_back(vpImagePoint((double)(solution[index_max][i].Y), (double)(solution[index_max][i].X))); } } else { roi_offset = roi; @@ -472,15 +447,10 @@ void vpMbtDistanceKltPoints::updateMask( } #else 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)) { + 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(i, j) = nb; } } else { @@ -492,22 +462,16 @@ void vpMbtDistanceKltPoints::updateMask( } } #else - unsigned char *ptrData = - (unsigned char *)mask->imageData + i_min * mask->widthStep + j_min; + 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)) { + 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++; @@ -533,8 +497,7 @@ void vpMbtDistanceKltPoints::updateMask( \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) +void vpMbtDistanceKltPoints::removeOutliers(const vpColVector &_w, const double &threshold_outlier) { std::map tmp; std::map tmp2; @@ -546,8 +509,7 @@ void vpMbtDistanceKltPoints::removeOutliers(const vpColVector &_w, 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()); + tmp[iter->first] = vpImagePoint(iter->second.get_i(), iter->second.get_j()); tmp2[iter->first] = curPointsInd[iter->first]; nbPointsCur++; } else { @@ -573,8 +535,7 @@ void vpMbtDistanceKltPoints::removeOutliers(const vpColVector &_w, \param _I : The image where to display. */ -void vpMbtDistanceKltPoints::displayPrimitive( - const vpImage &_I) +void vpMbtDistanceKltPoints::displayPrimitive(const vpImage &_I) { std::map::const_iterator iter = curPoints.begin(); for (; iter != curPoints.end(); ++iter) { @@ -617,11 +578,8 @@ void vpMbtDistanceKltPoints::displayPrimitive(const vpImage &_I) } } -void vpMbtDistanceKltPoints::display(const vpImage &I, - const vpHomogeneousMatrix & /*cMo*/, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, +void vpMbtDistanceKltPoints::display(const vpImage &I, const vpHomogeneousMatrix & /*cMo*/, + const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { if ((polygon->isVisible() && isTrackedKltPoints) || displayFullModel) { @@ -629,38 +587,26 @@ void vpMbtDistanceKltPoints::display(const vpImage &I, 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)) { + 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 > linesLst; if (useScanLine && !displayFullModel) - hiddenface->computeScanLineQuery( - roi[j].first, roi[(j + 1) % roi.size()].first, linesLst, true); + 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)); + 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); + 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); } @@ -669,11 +615,8 @@ void vpMbtDistanceKltPoints::display(const vpImage &I, } } -void vpMbtDistanceKltPoints::display(const vpImage &I, - const vpHomogeneousMatrix & /*cMo*/, - const vpCameraParameters &camera, - const vpColor &col, - const unsigned int thickness, +void vpMbtDistanceKltPoints::display(const vpImage &I, const vpHomogeneousMatrix & /*cMo*/, + const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { if ((polygon->isVisible() && isTrackedKltPoints) || displayFullModel) { @@ -681,38 +624,26 @@ void vpMbtDistanceKltPoints::display(const vpImage &I, 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)) { + 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 > linesLst; if (useScanLine && !displayFullModel) - hiddenface->computeScanLineQuery( - roi[j].first, roi[(j + 1) % roi.size()].first, linesLst, true); + 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)); + 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); + 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); } diff --git a/modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp b/modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp index 5f9083e45a..1b664a0e73 100644 --- a/modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp +++ b/modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp @@ -51,8 +51,8 @@ */ vpMbtKltXmlParser::vpMbtKltXmlParser() - : maskBorder(0), maxFeatures(0), winSize(0), qualityValue(0.), minDist(0.), - harrisParam(0.), blockSize(0), pyramidLevels(0) + : maskBorder(0), maxFeatures(0), winSize(0), qualityValue(0.), minDist(0.), harrisParam(0.), blockSize(0), + pyramidLevels(0) { init(); } @@ -116,11 +116,9 @@ void vpMbtKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) bool klt_node = false; bool lod_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case camera: { @@ -149,48 +147,32 @@ void vpMbtKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) } 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; + 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; + 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; + 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; + std::cout << "lod : min line length threshold : " << minLineLengthThreshold << " (default)" << std::endl; + std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << " (default)" << std::endl; } } @@ -213,11 +195,9 @@ void vpMbtKltXmlParser::read_klt(xmlDocPtr doc, xmlNodePtr node) bool size_block_node = false; bool pyramid_lvl_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case mask_border: { @@ -262,50 +242,42 @@ void vpMbtKltXmlParser::read_klt(xmlDocPtr doc, xmlNodePtr node) } if (!mask_border_node) - std::cout << "klt : Mask Border : " << maskBorder << " (default)" - << std::endl; + 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; + 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; + 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; + 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; + 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; + 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; + 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; + std::cout << "klt : Pyramid Levels : " << pyramidLevels << " (default)" << std::endl; else std::cout << "klt : Pyramid Levels : " << pyramidLevels << std::endl; } diff --git a/modules/tracker/mbt/src/vpMbGenericTracker.cpp b/modules/tracker/mbt/src/vpMbGenericTracker.cpp index 1c9109fff3..0b09fa4119 100644 --- a/modules/tracker/mbt/src/vpMbGenericTracker.cpp +++ b/modules/tracker/mbt/src/vpMbGenericTracker.cpp @@ -41,10 +41,8 @@ #include vpMbGenericTracker::vpMbGenericTracker() - : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), - m_mapOfFeatureFactors(), m_mapOfTrackers(), m_percentageGdPt(0.4), - m_referenceCameraName("Camera"), m_thresholdOutlier(0.5), m_w(), - m_weightedError() + : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(), + m_percentageGdPt(0.4), m_referenceCameraName("Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError() { m_mapOfTrackers["Camera"] = new TrackerWrapper(EDGE_TRACKER); @@ -54,8 +52,7 @@ vpMbGenericTracker::vpMbGenericTracker() // Add default ponderation between each feature type m_mapOfFeatureFactors[EDGE_TRACKER] = 1.0; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) m_mapOfFeatureFactors[KLT_TRACKER] = 1.0; #endif @@ -63,16 +60,12 @@ vpMbGenericTracker::vpMbGenericTracker() m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0; } -vpMbGenericTracker::vpMbGenericTracker(const unsigned int nbCameras, - const int trackerType) - : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), - m_mapOfFeatureFactors(), m_mapOfTrackers(), m_percentageGdPt(0.4), - m_referenceCameraName("Camera"), m_thresholdOutlier(0.5), m_w(), - m_weightedError() +vpMbGenericTracker::vpMbGenericTracker(const unsigned int nbCameras, const int trackerType) + : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(), + m_percentageGdPt(0.4), m_referenceCameraName("Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError() { if (nbCameras == 0) { - throw vpException(vpTrackingException::fatalError, - "Cannot use no camera!"); + throw vpException(vpTrackingException::fatalError, "Cannot use no camera!"); } else if (nbCameras == 1) { m_mapOfTrackers["Camera"] = new TrackerWrapper(trackerType); @@ -95,8 +88,7 @@ vpMbGenericTracker::vpMbGenericTracker(const unsigned int nbCameras, // Add default ponderation between each feature type m_mapOfFeatureFactors[EDGE_TRACKER] = 1.0; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) m_mapOfFeatureFactors[KLT_TRACKER] = 1.0; #endif @@ -105,10 +97,8 @@ vpMbGenericTracker::vpMbGenericTracker(const unsigned int nbCameras, } vpMbGenericTracker::vpMbGenericTracker(const std::vector &trackerTypes) - : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), - m_mapOfFeatureFactors(), m_mapOfTrackers(), m_percentageGdPt(0.4), - m_referenceCameraName("Camera"), m_thresholdOutlier(0.5), m_w(), - m_weightedError() + : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(), + m_percentageGdPt(0.4), m_referenceCameraName("Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError() { if (trackerTypes.empty()) { throw vpException(vpException::badValue, "There is no camera!"); @@ -136,8 +126,7 @@ vpMbGenericTracker::vpMbGenericTracker(const std::vector &trackerTypes) // Add default ponderation between each feature type m_mapOfFeatureFactors[EDGE_TRACKER] = 1.0; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) m_mapOfFeatureFactors[KLT_TRACKER] = 1.0; #endif @@ -145,18 +134,14 @@ vpMbGenericTracker::vpMbGenericTracker(const std::vector &trackerTypes) m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0; } -vpMbGenericTracker::vpMbGenericTracker( - const std::vector &cameraNames, - const std::vector &trackerTypes) - : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), - m_mapOfFeatureFactors(), m_mapOfTrackers(), m_percentageGdPt(0.4), - m_referenceCameraName("Camera"), m_thresholdOutlier(0.5), m_w(), - m_weightedError() +vpMbGenericTracker::vpMbGenericTracker(const std::vector &cameraNames, + const std::vector &trackerTypes) + : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(), + m_percentageGdPt(0.4), m_referenceCameraName("Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError() { if (cameraNames.size() != trackerTypes.size() || cameraNames.empty()) { - throw vpException( - vpTrackingException::badValue, - "cameraNames.size() != trackerTypes.size() || cameraNames.empty()"); + throw vpException(vpTrackingException::badValue, + "cameraNames.size() != trackerTypes.size() || cameraNames.empty()"); } for (size_t i = 0; i < cameraNames.size(); i++) { @@ -172,8 +157,7 @@ vpMbGenericTracker::vpMbGenericTracker( // Add default ponderation between each feature type m_mapOfFeatureFactors[EDGE_TRACKER] = 1.0; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) m_mapOfFeatureFactors[KLT_TRACKER] = 1.0; #endif @@ -183,9 +167,8 @@ vpMbGenericTracker::vpMbGenericTracker( vpMbGenericTracker::~vpMbGenericTracker() { - for (std::map::iterator it = - m_mapOfTrackers.begin(); - it != m_mapOfTrackers.end(); ++it) { + for (std::map::iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); + ++it) { delete it->second; it->second = NULL; } @@ -197,25 +180,21 @@ void vpMbGenericTracker::computeProjectionError() double rawTotalProjectionError = 0.0; unsigned int nbTotalFeaturesUsed = 0; - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; double curProjError = tracker->getProjectionError(); - unsigned int nbFeaturesUsed = - tracker->nbFeaturesForProjErrorComputation; + unsigned int nbFeaturesUsed = tracker->nbFeaturesForProjErrorComputation; if (nbFeaturesUsed > 0) { nbTotalFeaturesUsed += nbFeaturesUsed; - rawTotalProjectionError += - (vpMath::rad(curProjError) * nbFeaturesUsed); + rawTotalProjectionError += (vpMath::rad(curProjError) * nbFeaturesUsed); } } if (nbTotalFeaturesUsed > 0) { - projectionError = - vpMath::deg(rawTotalProjectionError / (double)nbTotalFeaturesUsed); + projectionError = vpMath::deg(rawTotalProjectionError / (double)nbTotalFeaturesUsed); } else { projectionError = 90.0; } @@ -224,14 +203,12 @@ void vpMbGenericTracker::computeProjectionError() } } -void vpMbGenericTracker::computeVVS( - std::map *> &mapOfImages) +void vpMbGenericTracker::computeVVS(std::map *> &mapOfImages) { computeVVSInit(mapOfImages); if (m_error.getRows() < 4) { - throw vpTrackingException(vpTrackingException::notEnoughPointError, - "Error: not enough features"); + throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features"); } double normRes = 0; @@ -253,8 +230,7 @@ void vpMbGenericTracker::computeVVS( // Create the map of VelocityTwistMatrices std::map mapOfVelocityTwist; - for (std::map::const_iterator it = - m_mapOfCameraTransformationMatrix.begin(); + for (std::map::const_iterator it = m_mapOfCameraTransformationMatrix.begin(); it != m_mapOfCameraTransformationMatrix.end(); ++it) { vpVelocityTwistMatrix cVo; cVo.buildFrom(it->second); @@ -262,34 +238,27 @@ void vpMbGenericTracker::computeVVS( } double factorEdge = m_mapOfFeatureFactors[EDGE_TRACKER]; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) double factorKlt = m_mapOfFeatureFactors[KLT_TRACKER]; #endif double factorDepth = m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER]; double factorDepthDense = m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER]; - while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && - (iter < m_maxIter)) { + while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) { computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist); bool reStartFromLastIncrement = false; - computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, - reStartFromLastIncrement); + computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement); if (reStartFromLastIncrement) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; - tracker->cMo = - m_mapOfCameraTransformationMatrix[it->first] * cMo_prev; + tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo_prev; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) vpHomogeneousMatrix c_curr_tTc_curr0 = - m_mapOfCameraTransformationMatrix[it->first] * cMo_prev * - tracker->c0Mo.inverse(); + m_mapOfCameraTransformationMatrix[it->first] * cMo_prev * tracker->c0Mo.inverse(); tracker->ctTc0 = c_curr_tTc_curr0; #endif } @@ -322,8 +291,7 @@ void vpMbGenericTracker::computeVVS( vpMatrix K; // kernel unsigned int rank = (m_L * cVo).kernel(K); if (rank == 0) { - throw vpException(vpException::fatalError, - "Rank=0, cannot estimate the pose !"); + throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !"); } if (rank != 6) { @@ -341,15 +309,13 @@ void vpMbGenericTracker::computeVVS( double den = 0; unsigned int start_index = 0; - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; if (tracker->m_trackerType & EDGE_TRACKER) { for (unsigned int i = 0; i < tracker->m_error_edge.getRows(); i++) { - double wi = - tracker->m_w_edge[i] * tracker->m_factor[i] * factorEdge; + double wi = tracker->m_w_edge[i] * tracker->m_factor[i] * factorEdge; W_true[start_index + i] = wi; m_weightedError[start_index + i] = wi * m_error[start_index + i]; @@ -364,8 +330,7 @@ void vpMbGenericTracker::computeVVS( start_index += tracker->m_error_edge.getRows(); } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (tracker->m_trackerType & KLT_TRACKER) { for (unsigned int i = 0; i < tracker->m_error_klt.getRows(); i++) { double wi = tracker->m_w_klt[i] * factorKlt; @@ -385,8 +350,7 @@ void vpMbGenericTracker::computeVVS( #endif if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) { - for (unsigned int i = 0; i < tracker->m_error_depthNormal.getRows(); - i++) { + for (unsigned int i = 0; i < tracker->m_error_depthNormal.getRows(); i++) { double wi = tracker->m_w_depthNormal[i] * factorDepth; m_weightedError[start_index + i] = wi * m_error[start_index + i]; @@ -402,8 +366,7 @@ void vpMbGenericTracker::computeVVS( } if (tracker->m_trackerType & DEPTH_DENSE_TRACKER) { - for (unsigned int i = 0; i < tracker->m_error_depthDense.getRows(); - i++) { + for (unsigned int i = 0; i < tracker->m_error_depthDense.getRows(); i++) { double wi = tracker->m_w_depthDense[i] * factorDepthDense; m_weightedError[start_index + i] = wi * m_error[start_index + i]; @@ -422,31 +385,25 @@ void vpMbGenericTracker::computeVVS( normRes_1 = normRes; normRes = sqrt(num / den); - computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, - m_weightedError, m_error, error_prev, LTR, mu, - v); + computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, m_weightedError, m_error, error_prev, LTR, mu, v); cMo_prev = cMo; cMo = vpExponentialMap::direct(v).inverse() * cMo; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; vpHomogeneousMatrix c_curr_tTc_curr0 = - m_mapOfCameraTransformationMatrix[it->first] * cMo * - tracker->c0Mo.inverse(); + m_mapOfCameraTransformationMatrix[it->first] * cMo * tracker->c0Mo.inverse(); tracker->ctTc0 = c_curr_tTc_curr0; } #endif // Update cMo - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo; @@ -456,11 +413,9 @@ void vpMbGenericTracker::computeVVS( iter++; } - computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, - LVJ_true, m_error); + computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; @@ -472,18 +427,14 @@ void vpMbGenericTracker::computeVVS( void vpMbGenericTracker::computeVVSInit() { - throw vpException( - vpException::fatalError, - "vpMbGenericTracker::computeVVSInit() should not be called!"); + throw vpException(vpException::fatalError, "vpMbGenericTracker::computeVVSInit() should not be called!"); } -void vpMbGenericTracker::computeVVSInit( - std::map *> &mapOfImages) +void vpMbGenericTracker::computeVVSInit(std::map *> &mapOfImages) { unsigned int nbFeatures = 0; - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->computeVVSInit(mapOfImages[it->first]); @@ -512,17 +463,13 @@ void vpMbGenericTracker::computeVVSInteractionMatrixAndResidu( { unsigned int start_index = 0; - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) - vpHomogeneousMatrix c_curr_tTc_curr0 = - m_mapOfCameraTransformationMatrix[it->first] * cMo * - tracker->c0Mo.inverse(); +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) + vpHomogeneousMatrix c_curr_tTc_curr0 = m_mapOfCameraTransformationMatrix[it->first] * cMo * tracker->c0Mo.inverse(); tracker->ctTc0 = c_curr_tTc_curr0; #endif @@ -539,8 +486,7 @@ void vpMbGenericTracker::computeVVSWeights() { unsigned int start_index = 0; - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->computeVVSWeights(); @@ -563,21 +509,16 @@ void vpMbGenericTracker::computeVVSWeights() \note This function will display the model only for the reference camera. */ -void vpMbGenericTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, - const vpColor &col, - const unsigned int thickness, +void vpMbGenericTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; tracker->display(I, cMo_, cam_, col, thickness, displayFullModel); } else { - std::cerr << "Cannot find the reference camera: " << m_referenceCameraName - << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } } @@ -594,21 +535,16 @@ void vpMbGenericTracker::display(const vpImage &I, \note This function will display the model only for the reference camera. */ -void vpMbGenericTracker::display(const vpImage &I, - const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &cam_, - const vpColor &col, - const unsigned int thickness, +void vpMbGenericTracker::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness, const bool displayFullModel) { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; tracker->display(I, cMo_, cam_, col, thickness, displayFullModel); } else { - std::cerr << "Cannot find the reference camera: " << m_referenceCameraName - << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } } @@ -628,24 +564,20 @@ void vpMbGenericTracker::display(const vpImage &I, \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::display( - const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, const vpCameraParameters &cam2, - const vpColor &color, const unsigned int thickness, - const bool displayFullModel) +void vpMbGenericTracker::display(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, + const vpCameraParameters &cam1, const vpCameraParameters &cam2, const vpColor &color, + const unsigned int thickness, const bool displayFullModel) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel); ++it; it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel); } else { - std::cerr - << "The tracker is not set as a stereo configuration! There are " - << m_mapOfTrackers.size() << " cameras!" << std::endl; + std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!" + << std::endl; } } @@ -665,24 +597,20 @@ void vpMbGenericTracker::display( \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::display( - const vpImage &I1, const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, - const vpCameraParameters &cam1, const vpCameraParameters &cam2, - const vpColor &color, const unsigned int thickness, - const bool displayFullModel) +void vpMbGenericTracker::display(const vpImage &I1, const vpImage &I2, const vpHomogeneousMatrix &c1Mo, + const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1, + const vpCameraParameters &cam2, const vpColor &color, const unsigned int thickness, + const bool displayFullModel) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel); ++it; it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel); } else { - std::cerr - << "The tracker is not set as a stereo configuration! There are " - << m_mapOfTrackers.size() << " cameras!" << std::endl; + std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!" + << std::endl; } } @@ -697,33 +625,24 @@ void vpMbGenericTracker::display( \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbGenericTracker::display( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness, - const bool displayFullModel) +void vpMbGenericTracker::display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, + const vpColor &col, const unsigned int thickness, const bool displayFullModel) { // Display only for the given images - for (std::map *>::const_iterator - it_img = mapOfImages.begin(); + for (std::map *>::const_iterator it_img = mapOfImages.begin(); it_img != mapOfImages.end(); ++it_img) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(it_img->first); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(it_img->first); - std::map::const_iterator it_cam = - mapOfCameraParameters.find(it_img->first); - - if (it_tracker != m_mapOfTrackers.end() && - it_camPose != mapOfCameraPoses.end() && + std::map::const_iterator it_tracker = m_mapOfTrackers.find(it_img->first); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first); + std::map::const_iterator it_cam = mapOfCameraParameters.find(it_img->first); + + if (it_tracker != m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() && it_cam != mapOfCameraParameters.end()) { TrackerWrapper *tracker = it_tracker->second; - tracker->display(*it_img->second, it_camPose->second, it_cam->second, - col, thickness, displayFullModel); + tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel); } else { - std::cerr << "Missing elements for image:" << it_img->first << "!" - << std::endl; + std::cerr << "Missing elements for image:" << it_img->first << "!" << std::endl; } } } @@ -739,33 +658,24 @@ void vpMbGenericTracker::display( \param displayFullModel : If true, the full model is displayed (even the non visible faces). */ -void vpMbGenericTracker::display( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses, - const std::map &mapOfCameraParameters, - const vpColor &col, const unsigned int thickness, - const bool displayFullModel) +void vpMbGenericTracker::display(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses, + const std::map &mapOfCameraParameters, + const vpColor &col, const unsigned int thickness, const bool displayFullModel) { // Display only for the given images - for (std::map *>::const_iterator it_img = - mapOfImages.begin(); + for (std::map *>::const_iterator it_img = mapOfImages.begin(); it_img != mapOfImages.end(); ++it_img) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(it_img->first); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(it_img->first); - std::map::const_iterator it_cam = - mapOfCameraParameters.find(it_img->first); - - if (it_tracker != m_mapOfTrackers.end() && - it_camPose != mapOfCameraPoses.end() && + std::map::const_iterator it_tracker = m_mapOfTrackers.find(it_img->first); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first); + std::map::const_iterator it_cam = mapOfCameraParameters.find(it_img->first); + + if (it_tracker != m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() && it_cam != mapOfCameraParameters.end()) { TrackerWrapper *tracker = it_tracker->second; - tracker->display(*it_img->second, it_camPose->second, it_cam->second, - col, thickness, displayFullModel); + tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel); } else { - std::cerr << "Missing elements for image:" << it_img->first << "!" - << std::endl; + std::cerr << "Missing elements for image:" << it_img->first << "!" << std::endl; } } } @@ -779,8 +689,7 @@ std::vector vpMbGenericTracker::getCameraNames() const { std::vector cameraNames; - for (std::map::const_iterator it_tracker = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) { cameraNames.push_back(it_tracker->first); } @@ -796,20 +705,17 @@ std::vector vpMbGenericTracker::getCameraNames() const \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::getCameraParameters(vpCameraParameters &cam1, - vpCameraParameters &cam2) const +void vpMbGenericTracker::getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->getCameraParameters(cam1); ++it; it->second->getCameraParameters(cam2); } else { - std::cerr - << "The tracker is not set as a stereo configuration! There are " - << m_mapOfTrackers.size() << " cameras!" << std::endl; + std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!" + << std::endl; } } @@ -818,14 +724,12 @@ void vpMbGenericTracker::getCameraParameters(vpCameraParameters &cam1, \param mapOfCameraParameters : Map of camera parameters. */ -void vpMbGenericTracker::getCameraParameters( - std::map &mapOfCameraParameters) const +void vpMbGenericTracker::getCameraParameters(std::map &mapOfCameraParameters) const { // Clear the input map mapOfCameraParameters.clear(); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { vpCameraParameters cam_; it->second->getCameraParameters(cam_); @@ -844,8 +748,7 @@ std::map vpMbGenericTracker::getCameraTrackerTypes() const std::map trackingTypes; TrackerWrapper *traker; - for (std::map::const_iterator it_tracker = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) { traker = it_tracker->second; trackingTypes[it_tracker->first] = traker->getTrackerType(); @@ -862,20 +765,17 @@ std::map vpMbGenericTracker::getCameraTrackerTypes() const \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::getClipping(unsigned int &clippingFlag1, - unsigned int &clippingFlag2) const +void vpMbGenericTracker::getClipping(unsigned int &clippingFlag1, unsigned int &clippingFlag2) const { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); clippingFlag1 = it->second->getClipping(); ++it; clippingFlag2 = it->second->getClipping(); } else { - std::cerr - << "The tracker is not set as a stereo configuration! There are " - << m_mapOfTrackers.size() << " cameras!" << std::endl; + std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!" + << std::endl; } } @@ -884,13 +784,11 @@ void vpMbGenericTracker::getClipping(unsigned int &clippingFlag1, \param mapOfClippingFlags : Map of clipping flags. */ -void vpMbGenericTracker::getClipping( - std::map &mapOfClippingFlags) const +void vpMbGenericTracker::getClipping(std::map &mapOfClippingFlags) const { mapOfClippingFlags.clear(); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; mapOfClippingFlags[it->first] = tracker->getClipping(); @@ -904,14 +802,12 @@ void vpMbGenericTracker::getClipping( */ vpMbHiddenFaces &vpMbGenericTracker::getFaces() { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { return it->second->getFaces(); } - std::cerr << "The reference camera: " << m_referenceCameraName - << " cannot be found!" << std::endl; + std::cerr << "The reference camera: " << m_referenceCameraName << " cannot be found!" << std::endl; return faces; } @@ -920,35 +816,29 @@ vpMbHiddenFaces &vpMbGenericTracker::getFaces() \return Reference to the face structure. */ -vpMbHiddenFaces & -vpMbGenericTracker::getFaces(const std::string &cameraName) +vpMbHiddenFaces &vpMbGenericTracker::getFaces(const std::string &cameraName) { - std::map::const_iterator it = - m_mapOfTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfTrackers.find(cameraName); if (it != m_mapOfTrackers.end()) { return it->second->getFaces(); } - std::cerr << "The camera: " << cameraName << " cannot be found!" - << std::endl; + std::cerr << "The camera: " << cameraName << " cannot be found!" << std::endl; return faces; } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) /*! Return the address of the circle feature list for the reference camera. */ std::list &vpMbGenericTracker::getFeaturesCircle() { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; return tracker->getFeaturesCircle(); } else { - throw vpException(vpTrackingException::badValue, - "Cannot find the reference camera: %s!", + throw vpException(vpTrackingException::badValue, "Cannot find the reference camera: %s!", m_referenceCameraName.c_str()); } } @@ -956,17 +846,14 @@ std::list &vpMbGenericTracker::getFeaturesCircle() /*! Return the address of the cylinder feature list for the reference camera. */ -std::list & -vpMbGenericTracker::getFeaturesKltCylinder() +std::list &vpMbGenericTracker::getFeaturesKltCylinder() { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; return tracker->getFeaturesKltCylinder(); } else { - throw vpException(vpTrackingException::badValue, - "Cannot find the reference camera: %s!", + throw vpException(vpTrackingException::badValue, "Cannot find the reference camera: %s!", m_referenceCameraName.c_str()); } } @@ -976,14 +863,12 @@ vpMbGenericTracker::getFeaturesKltCylinder() */ std::list &vpMbGenericTracker::getFeaturesKlt() { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; return tracker->getFeaturesKlt(); } else { - throw vpException(vpTrackingException::badValue, - "Cannot find the reference camera: %s!", + throw vpException(vpTrackingException::badValue, "Cannot find the reference camera: %s!", m_referenceCameraName.c_str()); } } @@ -998,13 +883,9 @@ std::list &vpMbGenericTracker::getFeaturesKlt() \sa setGoodMovingEdgesRatioThreshold() */ -double vpMbGenericTracker::getGoodMovingEdgesRatioThreshold() const -{ - return m_percentageGdPt; -} +double vpMbGenericTracker::getGoodMovingEdgesRatioThreshold() const { return m_percentageGdPt; } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) /*! Get the current list of KLT points for the reference camera. @@ -1015,14 +896,12 @@ double vpMbGenericTracker::getGoodMovingEdgesRatioThreshold() const */ std::vector vpMbGenericTracker::getKltImagePoints() const { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; return tracker->getKltImagePoints(); } else { - std::cerr << "Cannot find the reference camera: " << m_referenceCameraName - << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } return std::vector(); @@ -1036,17 +915,14 @@ std::vector vpMbGenericTracker::getKltImagePoints() const \return the list of KLT points and their id through vpKltOpencv. */ -std::map -vpMbGenericTracker::getKltImagePointsWithId() const +std::map vpMbGenericTracker::getKltImagePointsWithId() const { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; return tracker->getKltImagePointsWithId(); } else { - std::cerr << "Cannot find the reference camera: " << m_referenceCameraName - << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } return std::map(); @@ -1059,14 +935,12 @@ vpMbGenericTracker::getKltImagePointsWithId() const */ unsigned int vpMbGenericTracker::getKltMaskBorder() const { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; return tracker->getKltMaskBorder(); } else { - std::cerr << "Cannot find the reference camera: " << m_referenceCameraName - << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } return 0; @@ -1079,14 +953,12 @@ unsigned int vpMbGenericTracker::getKltMaskBorder() const */ int vpMbGenericTracker::getKltNbPoints() const { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; return tracker->getKltNbPoints(); } else { - std::cerr << "Cannot find the reference camera: " << m_referenceCameraName - << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } return 0; @@ -1099,16 +971,14 @@ int vpMbGenericTracker::getKltNbPoints() const */ vpKltOpencv vpMbGenericTracker::getKltOpencv() const { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName); if (it_tracker != m_mapOfTrackers.end()) { TrackerWrapper *tracker; tracker = it_tracker->second; return tracker->getKltOpencv(); } else { - std::cerr << "Cannot find the reference camera: " << m_referenceCameraName - << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } return vpKltOpencv(); @@ -1122,20 +992,17 @@ vpKltOpencv vpMbGenericTracker::getKltOpencv() const \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::getKltOpencv(vpKltOpencv &klt1, - vpKltOpencv &klt2) const +void vpMbGenericTracker::getKltOpencv(vpKltOpencv &klt1, vpKltOpencv &klt2) const { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); klt1 = it->second->getKltOpencv(); ++it; klt2 = it->second->getKltOpencv(); } else { - std::cerr - << "The tracker is not set as a stereo configuration! There are " - << m_mapOfTrackers.size() << " cameras!" << std::endl; + std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!" + << std::endl; } } @@ -1144,13 +1011,11 @@ void vpMbGenericTracker::getKltOpencv(vpKltOpencv &klt1, \param mapOfKlts : Map if klt trackers. */ -void vpMbGenericTracker::getKltOpencv( - std::map &mapOfKlts) const +void vpMbGenericTracker::getKltOpencv(std::map &mapOfKlts) const { mapOfKlts.clear(); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; mapOfKlts[it->first] = tracker->getKltOpencv(); @@ -1165,14 +1030,12 @@ void vpMbGenericTracker::getKltOpencv( */ std::vector vpMbGenericTracker::getKltPoints() const { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; return tracker->getKltPoints(); } else { - std::cerr << "Cannot find the reference camera: " << m_referenceCameraName - << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } return std::vector(); @@ -1185,10 +1048,7 @@ std::vector vpMbGenericTracker::getKltPoints() const \return threshold_outlier : Threshold for the weight below which a point is rejected. */ -double vpMbGenericTracker::getKltThresholdAcceptation() const -{ - return m_thresholdOutlier; -} +double vpMbGenericTracker::getKltThresholdAcceptation() const { return m_thresholdOutlier; } #endif /*! @@ -1205,18 +1065,14 @@ double vpMbGenericTracker::getKltThresholdAcceptation() const \note Multi-scale moving edge tracking is not possible, scale level=0 must be used. */ -void vpMbGenericTracker::getLcircle( - const std::string &cameraName, - std::list &circlesList, - const unsigned int level) const +void vpMbGenericTracker::getLcircle(const std::string &cameraName, std::list &circlesList, + const unsigned int level) const { - std::map::const_iterator it = - m_mapOfTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfTrackers.find(cameraName); if (it != m_mapOfTrackers.end()) { it->second->getLcircle(circlesList, level); } else { - std::cerr << "The camera: " << cameraName << " does not exist!" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl; } } @@ -1234,18 +1090,14 @@ void vpMbGenericTracker::getLcircle( \note Multi-scale moving edge tracking is not possible, scale level=0 must be used. */ -void vpMbGenericTracker::getLcylinder( - const std::string &cameraName, - std::list &cylindersList, - const unsigned int level) const +void vpMbGenericTracker::getLcylinder(const std::string &cameraName, std::list &cylindersList, + const unsigned int level) const { - std::map::const_iterator it = - m_mapOfTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfTrackers.find(cameraName); if (it != m_mapOfTrackers.end()) { it->second->getLcylinder(cylindersList, level); } else { - std::cerr << "The camera: " << cameraName << " does not exist!" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl; } } @@ -1263,17 +1115,14 @@ void vpMbGenericTracker::getLcylinder( \note Multi-scale moving edge tracking is not possible, scale level=0 must be used. */ -void vpMbGenericTracker::getLline(const std::string &cameraName, - std::list &linesList, +void vpMbGenericTracker::getLline(const std::string &cameraName, std::list &linesList, const unsigned int level) const { - std::map::const_iterator it = - m_mapOfTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfTrackers.find(cameraName); if (it != m_mapOfTrackers.end()) { it->second->getLline(linesList, level); } else { - std::cerr << "The camera: " << cameraName << " does not exist!" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl; } } @@ -1284,14 +1133,12 @@ void vpMbGenericTracker::getLline(const std::string &cameraName, */ vpMe vpMbGenericTracker::getMovingEdge() const { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { return it->second->getMovingEdge(); } else { - std::cerr << "The reference camera: " << m_referenceCameraName - << " does not exist!" << std::endl; + std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl; } return vpMe(); @@ -1308,21 +1155,18 @@ vpMe vpMbGenericTracker::getMovingEdge() const void vpMbGenericTracker::getMovingEdge(vpMe &me1, vpMe &me2) const { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->getMovingEdge(me1); ++it; it->second->getMovingEdge(me2); } else { - std::cerr - << "The tracker is not set as a stereo configuration! There are " - << m_mapOfTrackers.size() << " cameras!" << std::endl; + std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!" + << std::endl; } } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) #endif @@ -1331,13 +1175,11 @@ void vpMbGenericTracker::getMovingEdge(vpMe &me1, vpMe &me2) const \param mapOfMovingEdges : Map of moving edge parameters for all the cameras. */ -void vpMbGenericTracker::getMovingEdge( - std::map &mapOfMovingEdges) const +void vpMbGenericTracker::getMovingEdge(std::map &mapOfMovingEdges) const { mapOfMovingEdges.clear(); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; mapOfMovingEdges[it->first] = tracker->getMovingEdge(); @@ -1361,16 +1203,14 @@ void vpMbGenericTracker::getMovingEdge( */ unsigned int vpMbGenericTracker::getNbPoints(const unsigned int level) const { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); unsigned int nbGoodPoints = 0; if (it != m_mapOfTrackers.end()) { nbGoodPoints = it->second->getNbPoints(level); } else { - std::cerr << "The reference camera: " << m_referenceCameraName - << " does not exist!" << std::endl; + std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl; } return nbGoodPoints; @@ -1390,14 +1230,11 @@ unsigned int vpMbGenericTracker::getNbPoints(const unsigned int level) const \note Multi-scale moving edge tracking is not possible, scale level=0 must be used. */ -void vpMbGenericTracker::getNbPoints( - std::map &mapOfNbPoints, - const unsigned int level) const +void vpMbGenericTracker::getNbPoints(std::map &mapOfNbPoints, const unsigned int level) const { mapOfNbPoints.clear(); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; mapOfNbPoints[it->first] = tracker->getNbPoints(level); @@ -1411,14 +1248,12 @@ void vpMbGenericTracker::getNbPoints( */ unsigned int vpMbGenericTracker::getNbPolygon() const { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { return it->second->getNbPolygon(); } - std::cerr << "The reference camera: " << m_referenceCameraName - << " does not exist!" << std::endl; + std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl; return 0; } @@ -1427,13 +1262,11 @@ unsigned int vpMbGenericTracker::getNbPolygon() const \return Number of polygons for all the cameras. */ -void vpMbGenericTracker::getNbPolygon( - std::map &mapOfNbPolygons) const +void vpMbGenericTracker::getNbPolygon(std::map &mapOfNbPolygons) const { mapOfNbPolygons.clear(); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; mapOfNbPolygons[it->first] = tracker->getNbPolygon(); @@ -1452,14 +1285,12 @@ void vpMbGenericTracker::getNbPolygon( */ vpMbtPolygon *vpMbGenericTracker::getPolygon(const unsigned int index) { - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { return it->second->getPolygon(index); } - std::cerr << "The reference camera: " << m_referenceCameraName - << " does not exist!" << std::endl; + std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl; return NULL; } @@ -1474,17 +1305,14 @@ vpMbtPolygon *vpMbGenericTracker::getPolygon(const unsigned int index) \return Pointer to the polygon index for the specified camera or NULL in case of problem. */ -vpMbtPolygon *vpMbGenericTracker::getPolygon(const std::string &cameraName, - const unsigned int index) +vpMbtPolygon *vpMbGenericTracker::getPolygon(const std::string &cameraName, const unsigned int index) { - std::map::const_iterator it = - m_mapOfTrackers.find(cameraName); + std::map::const_iterator it = m_mapOfTrackers.find(cameraName); if (it != m_mapOfTrackers.end()) { return it->second->getPolygon(index); } - std::cerr << "The camera: " << cameraName << " does not exist!" - << std::endl; + std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl; return NULL; } @@ -1504,22 +1332,16 @@ vpMbtPolygon *vpMbGenericTracker::getPolygon(const std::string &cameraName, only for the reference camera. */ std::pair, std::vector > > -vpMbGenericTracker::getPolygonFaces(const bool orderPolygons, - const bool useVisibility, - const bool clipPolygon) +vpMbGenericTracker::getPolygonFaces(const bool orderPolygons, const bool useVisibility, const bool clipPolygon) { - std::pair, std::vector > > - polygonFaces; + std::pair, std::vector > > polygonFaces; - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; - polygonFaces = - tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon); + polygonFaces = tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon); } else { - std::cerr << "Cannot find the reference camera: " << m_referenceCameraName - << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } return polygonFaces; @@ -1542,22 +1364,18 @@ vpMbGenericTracker::getPolygonFaces(const bool orderPolygons, \note This function will return the 2D polygons faces and 3D face points only for all the cameras. */ -void vpMbGenericTracker::getPolygonFaces( - std::map > &mapOfPolygons, - std::map > > &mapOfPoints, - const bool orderPolygons, const bool useVisibility, - const bool clipPolygon) +void vpMbGenericTracker::getPolygonFaces(std::map > &mapOfPolygons, + std::map > > &mapOfPoints, + const bool orderPolygons, const bool useVisibility, const bool clipPolygon) { mapOfPolygons.clear(); mapOfPoints.clear(); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; - std::pair, std::vector > > - polygonFaces = tracker->getPolygonFaces(orderPolygons, useVisibility, - clipPolygon); + std::pair, std::vector > > polygonFaces = + tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon); mapOfPolygons[it->first] = polygonFaces.first; mapOfPoints[it->first] = polygonFaces.second; @@ -1572,20 +1390,17 @@ void vpMbGenericTracker::getPolygonFaces( \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::getPose(vpHomogeneousMatrix &c1Mo, - vpHomogeneousMatrix &c2Mo) const +void vpMbGenericTracker::getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->getPose(c1Mo); ++it; it->second->getPose(c2Mo); } else { - std::cerr - << "The tracker is not set as a stereo configuration! There are " - << m_mapOfTrackers.size() << " cameras!" << std::endl; + std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!" + << std::endl; } } @@ -1594,14 +1409,12 @@ void vpMbGenericTracker::getPose(vpHomogeneousMatrix &c1Mo, \param mapOfCameraPoses : The map of camera poses for all the cameras. */ -void vpMbGenericTracker::getPose( - std::map &mapOfCameraPoses) const +void vpMbGenericTracker::getPose(std::map &mapOfCameraPoses) const { // Clear the map mapOfCameraPoses.clear(); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->getPose(mapOfCameraPoses[it->first]); @@ -1610,8 +1423,7 @@ void vpMbGenericTracker::getPose( void vpMbGenericTracker::init(const vpImage &I) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo; @@ -1619,15 +1431,10 @@ void vpMbGenericTracker::init(const vpImage &I) } } -void vpMbGenericTracker::initCircle(const vpPoint & /*p1*/, - const vpPoint & /*p2*/, - const vpPoint & /*p3*/, - const double /*radius*/, - const int /*idFace*/, - const std::string & /*name*/) +void vpMbGenericTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/, + const double /*radius*/, const int /*idFace*/, const std::string & /*name*/) { - throw vpException(vpException::fatalError, - "vpMbGenericTracker::initCircle() should not be called!"); + throw vpException(vpException::fatalError, "vpMbGenericTracker::initCircle() should not be called!"); } #ifdef VISP_HAVE_MODULE_GUI @@ -1653,10 +1460,12 @@ void vpMbGenericTracker::initCircle(const vpPoint & /*p1*/, \param I2 : Input image for the second camera. \param initFile1 : File containing the coordinates of at least 4 3D points the user has to click in the image acquired by the first camera. This file - should have .init extension (ie teabox.init). \param initFile2 : File + should have .init extension (ie teabox.init). + \param initFile2 : File containing the coordinates of at least 4 3D points the user has to click in the image acquired by the second camera. This file should have .init - extension. \param displayHelp : Optionnal display of an image that should + extension. + \param displayHelp : Optionnal display of an image that should have the same generic name as the init file (ie teabox.ppm). This image may be used to show where to click. This functionality is only available if visp_io module is used. @@ -1666,15 +1475,11 @@ void vpMbGenericTracker::initCircle(const vpPoint & /*p1*/, \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::initClick(const vpImage &I1, - const vpImage &I2, - const std::string &initFile1, - const std::string &initFile2, - const bool displayHelp) +void vpMbGenericTracker::initClick(const vpImage &I1, const vpImage &I2, + const std::string &initFile1, const std::string &initFile2, const bool displayHelp) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); TrackerWrapper *tracker = it->second; tracker->initClick(I1, initFile1, displayHelp); @@ -1691,10 +1496,8 @@ void vpMbGenericTracker::initClick(const vpImage &I1, tracker->getPose(cMo); } } else { - throw vpException( - vpTrackingException::initializationError, - "Cannot initClick()! Require two cameras but there are %d cameras!", - m_mapOfTrackers.size()); + throw vpException(vpTrackingException::initializationError, + "Cannot initClick()! Require two cameras but there are %d cameras!", m_mapOfTrackers.size()); } } @@ -1735,40 +1538,32 @@ void vpMbGenericTracker::initClick(const vpImage &I1, the known geometric transformation between each camera (see setCameraTransformationMatrix()). */ -void vpMbGenericTracker::initClick( - const std::map *> &mapOfImages, - const std::map &mapOfInitFiles, - const bool displayHelp) -{ - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_initFile = - mapOfInitFiles.find(m_referenceCameraName); - - if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && - it_initFile != mapOfInitFiles.end()) { +void vpMbGenericTracker::initClick(const std::map *> &mapOfImages, + const std::map &mapOfInitFiles, const bool displayHelp) +{ + std::map::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_initFile = mapOfInitFiles.find(m_referenceCameraName); + + if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) { TrackerWrapper *tracker = it_tracker->second; tracker->initClick(*it_img->second, it_initFile->second, displayHelp); tracker->getPose(cMo); } else { - throw vpException(vpTrackingException::initializationError, - "Cannot initClick for the reference camera!"); + throw vpException(vpTrackingException::initializationError, "Cannot initClick for the reference camera!"); } // Vector of missing initFile for cameras std::vector vectorOfMissingCameraPoses; // Set pose for the specified cameras - for (it_tracker = m_mapOfTrackers.begin(); - it_tracker != m_mapOfTrackers.end(); ++it_tracker) { + for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) { if (it_tracker->first != m_referenceCameraName) { it_img = mapOfImages.find(it_tracker->first); it_initFile = mapOfInitFiles.find(it_tracker->first); - if (it_img != mapOfImages.end() && - it_initFile != mapOfInitFiles.end()) { + if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) { // InitClick for the current camera TrackerWrapper *tracker = it_tracker->second; tracker->initClick(*it_img->second, it_initFile->second, displayHelp); @@ -1779,15 +1574,13 @@ void vpMbGenericTracker::initClick( } // Init for cameras that do not have an initFile - for (std::vector::const_iterator it = - vectorOfMissingCameraPoses.begin(); + for (std::vector::const_iterator it = vectorOfMissingCameraPoses.begin(); it != vectorOfMissingCameraPoses.end(); ++it) { it_img = mapOfImages.find(*it); std::map::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; m_mapOfTrackers[*it]->cMo = cCurrentMo; m_mapOfTrackers[*it]->init(*it_img->second); @@ -1801,29 +1594,20 @@ void vpMbGenericTracker::initClick( } #endif -void vpMbGenericTracker::initCylinder(const vpPoint & /*p1*/, - const vpPoint & /*p2*/, - const double /*radius*/, - const int /*idFace*/, - const std::string & /*name*/) +void vpMbGenericTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const double /*radius*/, + const int /*idFace*/, const std::string & /*name*/) { - throw vpException( - vpException::fatalError, - "vpMbGenericTracker::initCylinder() should not be called!"); + throw vpException(vpException::fatalError, "vpMbGenericTracker::initCylinder() should not be called!"); } void vpMbGenericTracker::initFaceFromCorners(vpMbtPolygon & /*polygon*/) { - throw vpException( - vpException::fatalError, - "vpMbGenericTracker::initFaceFromCorners() should not be called!"); + throw vpException(vpException::fatalError, "vpMbGenericTracker::initFaceFromCorners() should not be called!"); } void vpMbGenericTracker::initFaceFromLines(vpMbtPolygon & /*polygon*/) { - throw vpException( - vpException::fatalError, - "vpMbGenericTracker::initFaceFromLines() should not be called!"); + throw vpException(vpException::fatalError, "vpMbGenericTracker::initFaceFromLines() should not be called!"); } /*! @@ -1832,36 +1616,34 @@ void vpMbGenericTracker::initFaceFromLines(vpMbtPolygon & /*polygon*/) are allowed. 3D point coordinates are expressed in meter in the object frame with X, Y and Z values. 2D point coordinates are expressied in pixel coordinates, with first the line and then the column of the pixel in the - image. The structure of this file is the following. \code # 3D point - coordinates 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 # / - # corresponding 2D point coordinates - 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 # / + image. The structure of this file is the following. + \code + # 3D point coordinates + 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 # / + # corresponding 2D point coordinates + 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 I1 : Input image for the first camera. \param I2 : Input image for the second camera. \param initFile1 : Path to the file containing all the points for the first - camera. \param initFile2 : Path to the file containing all the points for + camera. + \param initFile2 : Path to the file containing all the points for the second camera. \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::initFromPoints(const vpImage &I1, - const vpImage &I2, - const std::string &initFile1, - const std::string &initFile2) +void vpMbGenericTracker::initFromPoints(const vpImage &I1, const vpImage &I2, + const std::string &initFile1, const std::string &initFile2) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); TrackerWrapper *tracker = it->second; tracker->initFromPoints(I1, initFile1); @@ -1888,56 +1670,46 @@ void vpMbGenericTracker::initFromPoints(const vpImage &I1, } } -void vpMbGenericTracker::initFromPoints( - const std::map *> &mapOfImages, - const std::map &mapOfInitPoints) +void vpMbGenericTracker::initFromPoints(const std::map *> &mapOfImages, + const std::map &mapOfInitPoints) { // Set the reference cMo - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_initPoints = - mapOfInitPoints.find(m_referenceCameraName); - - if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && - it_initPoints != mapOfInitPoints.end()) { + std::map::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_initPoints = mapOfInitPoints.find(m_referenceCameraName); + + if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) { TrackerWrapper *tracker = it_tracker->second; tracker->initFromPoints(*it_img->second, it_initPoints->second); tracker->getPose(cMo); } else { - throw vpException(vpTrackingException::initializationError, - "Cannot initFromPoints() for the reference camera!"); + throw vpException(vpTrackingException::initializationError, "Cannot initFromPoints() for the reference camera!"); } // Vector of missing initPoints for cameras std::vector vectorOfMissingCameraPoints; // Set pose for the specified cameras - for (it_tracker = m_mapOfTrackers.begin(); - it_tracker != m_mapOfTrackers.end(); ++it_tracker) { + for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) { it_img = mapOfImages.find(it_tracker->first); it_initPoints = mapOfInitPoints.find(it_tracker->first); - if (it_img != mapOfImages.end() && - it_initPoints != mapOfInitPoints.end()) { + if (it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) { // Set pose - it_tracker->second->initFromPoints(*it_img->second, - it_initPoints->second); + it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second); } else { vectorOfMissingCameraPoints.push_back(it_tracker->first); } } - for (std::vector::const_iterator it = - vectorOfMissingCameraPoints.begin(); + for (std::vector::const_iterator it = vectorOfMissingCameraPoints.begin(); it != vectorOfMissingCameraPoints.end(); ++it) { it_img = mapOfImages.find(*it); std::map::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo); } else { @@ -1960,14 +1732,11 @@ void vpMbGenericTracker::initFromPoints( \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::initFromPose(const vpImage &I1, - const vpImage &I2, - const std::string &initFile1, - const std::string &initFile2) +void vpMbGenericTracker::initFromPose(const vpImage &I1, const vpImage &I2, + const std::string &initFile1, const std::string &initFile2) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); TrackerWrapper *tracker = it->second; tracker->initFromPose(I1, initFile1); @@ -2008,34 +1777,28 @@ void vpMbGenericTracker::initFromPose(const vpImage &I1, the known geometric transformation between each camera (see setCameraTransformationMatrix()). */ -void vpMbGenericTracker::initFromPose( - const std::map *> &mapOfImages, - const std::map &mapOfInitPoses) +void vpMbGenericTracker::initFromPose(const std::map *> &mapOfImages, + const std::map &mapOfInitPoses) { // Set the reference cMo - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_initPose = - mapOfInitPoses.find(m_referenceCameraName); - - if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && - it_initPose != mapOfInitPoses.end()) { + std::map::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_initPose = mapOfInitPoses.find(m_referenceCameraName); + + if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) { TrackerWrapper *tracker = it_tracker->second; tracker->initFromPose(*it_img->second, it_initPose->second); tracker->getPose(cMo); } else { - throw vpException(vpTrackingException::initializationError, - "Cannot initFromPose() for the reference camera!"); + throw vpException(vpTrackingException::initializationError, "Cannot initFromPose() for the reference camera!"); } // Vector of missing pose matrices for cameras std::vector vectorOfMissingCameraPoses; // Set pose for the specified cameras - for (it_tracker = m_mapOfTrackers.begin(); - it_tracker != m_mapOfTrackers.end(); ++it_tracker) { + for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) { it_img = mapOfImages.find(it_tracker->first); it_initPose = mapOfInitPoses.find(it_tracker->first); @@ -2047,15 +1810,13 @@ void vpMbGenericTracker::initFromPose( } } - for (std::vector::const_iterator it = - vectorOfMissingCameraPoses.begin(); + for (std::vector::const_iterator it = vectorOfMissingCameraPoses.begin(); it != vectorOfMissingCameraPoses.end(); ++it) { it_img = mapOfImages.find(*it); std::map::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo); } else { @@ -2077,14 +1838,11 @@ void vpMbGenericTracker::initFromPose( \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::initFromPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo) +void vpMbGenericTracker::initFromPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->initFromPose(I1, c1Mo); ++it; @@ -2093,10 +1851,8 @@ void vpMbGenericTracker::initFromPose(const vpImage &I1, this->cMo = c1Mo; } else { - throw vpException( - vpTrackingException::initializationError, - "This method requires 2 cameras but there are %d cameras!", - m_mapOfTrackers.size()); + throw vpException(vpTrackingException::initializationError, + "This method requires 2 cameras but there are %d cameras!", m_mapOfTrackers.size()); } } @@ -2113,34 +1869,28 @@ void vpMbGenericTracker::initFromPose(const vpImage &I1, the known geometric transformation between each camera (see setCameraTransformationMatrix()). */ -void vpMbGenericTracker::initFromPose( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses) +void vpMbGenericTracker::initFromPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses) { // Set the reference cMo - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(m_referenceCameraName); - - if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && - it_camPose != mapOfCameraPoses.end()) { + std::map::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName); + + if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) { TrackerWrapper *tracker = it_tracker->second; tracker->initFromPose(*it_img->second, it_camPose->second); tracker->getPose(cMo); } else { - throw vpException(vpTrackingException::initializationError, - "Cannot set pose for the reference camera!"); + throw vpException(vpTrackingException::initializationError, "Cannot set pose for the reference camera!"); } // Vector of missing pose matrices for cameras std::vector vectorOfMissingCameraPoses; // Set pose for the specified cameras - for (it_tracker = m_mapOfTrackers.begin(); - it_tracker != m_mapOfTrackers.end(); ++it_tracker) { + for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) { it_img = mapOfImages.find(it_tracker->first); it_camPose = mapOfCameraPoses.find(it_tracker->first); @@ -2152,15 +1902,13 @@ void vpMbGenericTracker::initFromPose( } } - for (std::vector::const_iterator it = - vectorOfMissingCameraPoses.begin(); + for (std::vector::const_iterator it = vectorOfMissingCameraPoses.begin(); it != vectorOfMissingCameraPoses.end(); ++it) { it_img = mapOfImages.find(*it); std::map::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo); } else { @@ -2189,24 +1937,19 @@ void vpMbGenericTracker::initFromPose( */ void vpMbGenericTracker::loadConfigFile(const std::string &configFile) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->loadConfigFile(configFile); } if (m_mapOfTrackers.find(m_referenceCameraName) == m_mapOfTrackers.end()) { - throw vpException(vpException::fatalError, - "Cannot find the reference camera: %s!", - m_referenceCameraName.c_str()); + throw vpException(vpException::fatalError, "Cannot find the reference camera: %s!", m_referenceCameraName.c_str()); } m_mapOfTrackers[m_referenceCameraName]->getCameraParameters(this->cam); - this->angleAppears = - m_mapOfTrackers[m_referenceCameraName]->getAngleAppear(); - this->angleDisappears = - m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear(); + this->angleAppears = m_mapOfTrackers[m_referenceCameraName]->getAngleAppear(); + this->angleDisappears = m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear(); this->clippingFlag = m_mapOfTrackers[m_referenceCameraName]->getClipping(); } @@ -2225,16 +1968,13 @@ void vpMbGenericTracker::loadConfigFile(const std::string &configFile) \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::loadConfigFile(const std::string &configFile1, - const std::string &configFile2) +void vpMbGenericTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2) { if (m_mapOfTrackers.size() != 2) { - throw vpException(vpException::fatalError, - "The tracker is not set in a stereo configuration!"); + throw vpException(vpException::fatalError, "The tracker is not set in a stereo configuration!"); } - std::map::const_iterator it_tracker = - m_mapOfTrackers.begin(); + std::map::const_iterator it_tracker = m_mapOfTrackers.begin(); TrackerWrapper *tracker = it_tracker->second; tracker->loadConfigFile(configFile1); @@ -2243,16 +1983,12 @@ void vpMbGenericTracker::loadConfigFile(const std::string &configFile1, tracker->loadConfigFile(configFile2); if (m_mapOfTrackers.find(m_referenceCameraName) == m_mapOfTrackers.end()) { - throw vpException(vpException::fatalError, - "Cannot find the reference camera: %s!", - m_referenceCameraName.c_str()); + throw vpException(vpException::fatalError, "Cannot find the reference camera: %s!", m_referenceCameraName.c_str()); } m_mapOfTrackers[m_referenceCameraName]->getCameraParameters(this->cam); - this->angleAppears = - m_mapOfTrackers[m_referenceCameraName]->getAngleAppear(); - this->angleDisappears = - m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear(); + this->angleAppears = m_mapOfTrackers[m_referenceCameraName]->getAngleAppear(); + this->angleDisappears = m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear(); this->clippingFlag = m_mapOfTrackers[m_referenceCameraName]->getClipping(); } @@ -2270,28 +2006,23 @@ void vpMbGenericTracker::loadConfigFile(const std::string &configFile1, \note Configuration files must be supplied for all the cameras. */ -void vpMbGenericTracker::loadConfigFile( - const std::map &mapOfConfigFiles) +void vpMbGenericTracker::loadConfigFile(const std::map &mapOfConfigFiles) { - for (std::map::const_iterator it_tracker = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) { TrackerWrapper *tracker = it_tracker->second; - std::map::const_iterator it_config = - mapOfConfigFiles.find(it_tracker->first); + std::map::const_iterator it_config = mapOfConfigFiles.find(it_tracker->first); if (it_config != mapOfConfigFiles.end()) { tracker->loadConfigFile(it_config->second); } else { - throw vpException(vpTrackingException::initializationError, - "Missing configuration file for camera: %s!", + throw vpException(vpTrackingException::initializationError, "Missing configuration file for camera: %s!", it_tracker->first.c_str()); } } // Set the reference camera parameters - std::map::iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; tracker->getCameraParameters(cam); @@ -2301,8 +2032,7 @@ void vpMbGenericTracker::loadConfigFile( this->angleAppears = tracker->getAngleAppear(); this->angleDisappears = tracker->getAngleDisappear(); } else { - throw vpException(vpTrackingException::initializationError, - "The reference camera: %s does not exist!", + throw vpException(vpTrackingException::initializationError, "The reference camera: %s does not exist!", m_referenceCameraName.c_str()); } } @@ -2335,11 +2065,9 @@ CAO model files which include other CAO model files. \note All the trackers will use the same model in case of stereo / multiple cameras configuration. */ -void vpMbGenericTracker::loadModel(const std::string &modelFile, - const bool verbose) +void vpMbGenericTracker::loadModel(const std::string &modelFile, const bool verbose) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->loadModel(modelFile, verbose); @@ -2375,17 +2103,13 @@ which include other CAO model files. \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::loadModel(const std::string &modelFile1, - const std::string &modelFile2, - const bool verbose) +void vpMbGenericTracker::loadModel(const std::string &modelFile1, const std::string &modelFile2, const bool verbose) { if (m_mapOfTrackers.size() != 2) { - throw vpException(vpException::fatalError, - "The tracker is not set in a stereo configuration!"); + throw vpException(vpException::fatalError, "The tracker is not set in a stereo configuration!"); } - std::map::const_iterator it_tracker = - m_mapOfTrackers.begin(); + std::map::const_iterator it_tracker = m_mapOfTrackers.begin(); TrackerWrapper *tracker = it_tracker->second; tracker->loadModel(modelFile1, verbose); @@ -2421,35 +2145,27 @@ CAO model files which include other CAO model files. \note Each camera must have a model file. */ -void vpMbGenericTracker::loadModel( - const std::map &mapOfModelFiles, - const bool verbose) +void vpMbGenericTracker::loadModel(const std::map &mapOfModelFiles, const bool verbose) { - for (std::map::const_iterator it_tracker = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) { - std::map::const_iterator it_model = - mapOfModelFiles.find(it_tracker->first); + std::map::const_iterator it_model = mapOfModelFiles.find(it_tracker->first); if (it_model != mapOfModelFiles.end()) { TrackerWrapper *tracker = it_tracker->second; tracker->loadModel(it_model->second, verbose); } else { - throw vpException(vpTrackingException::initializationError, - "Cannot load model for camera: %s", + throw vpException(vpTrackingException::initializationError, "Cannot load model for camera: %s", it_tracker->first.c_str()); } } } #ifdef VISP_HAVE_PCL -void vpMbGenericTracker::preTracking( - std::map *> &mapOfImages, - std::map::ConstPtr> - &mapOfPointClouds) +void vpMbGenericTracker::preTracking(std::map *> &mapOfImages, + std::map::ConstPtr> &mapOfPointClouds) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first]); @@ -2457,18 +2173,15 @@ void vpMbGenericTracker::preTracking( } #endif -void vpMbGenericTracker::preTracking( - std::map *> &mapOfImages, - std::map *> &mapOfPointClouds, - std::map &mapOfPointCloudWidths, - std::map &mapOfPointCloudHeights) +void vpMbGenericTracker::preTracking(std::map *> &mapOfImages, + std::map *> &mapOfPointClouds, + std::map &mapOfPointCloudWidths, + std::map &mapOfPointCloudHeights) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; - tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first], - mapOfPointCloudWidths[it->first], + tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]); } } @@ -2482,20 +2195,15 @@ void vpMbGenericTracker::preTracking( model. \param verbose : verbose option to print additional information when loading CAO model files which include other CAO model files. */ -void vpMbGenericTracker::reInitModel(const vpImage &I, - const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, - const bool verbose) +void vpMbGenericTracker::reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose) { if (m_mapOfTrackers.size() != 1) { - throw vpException( - vpTrackingException::fatalError, - "This method requires exactly one camera, there are %d cameras!", - m_mapOfTrackers.size()); + throw vpException(vpTrackingException::fatalError, "This method requires exactly one camera, there are %d cameras!", + m_mapOfTrackers.size()); } - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName); if (it_tracker != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it_tracker->second; tracker->reInitModel(I, cad_name, cMo_, verbose); @@ -2503,8 +2211,7 @@ void vpMbGenericTracker::reInitModel(const vpImage &I, // Set reference pose tracker->getPose(cMo); } else { - throw vpException(vpTrackingException::fatalError, - "Cannot reInitModel() the reference camera!"); + throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() the reference camera!"); } modelInitialised = true; @@ -2525,17 +2232,13 @@ void vpMbGenericTracker::reInitModel(const vpImage &I, \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::reInitModel(const vpImage &I1, - const vpImage &I2, - const std::string &cad_name1, - const std::string &cad_name2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo, +void vpMbGenericTracker::reInitModel(const vpImage &I1, const vpImage &I2, + const std::string &cad_name1, const std::string &cad_name2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool verbose) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.begin(); + std::map::const_iterator it_tracker = m_mapOfTrackers.begin(); it_tracker->second->reInitModel(I1, cad_name1, c1Mo, verbose); @@ -2549,8 +2252,7 @@ void vpMbGenericTracker::reInitModel(const vpImage &I1, it_tracker->second->getPose(cMo); } } else { - throw vpException(vpTrackingException::fatalError, - "This method requires exactly two cameras!"); + throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras!"); } modelInitialised = true; @@ -2566,57 +2268,46 @@ void vpMbGenericTracker::reInitModel(const vpImage &I1, additional information when loading CAO model files which include other CAO model files. */ -void vpMbGenericTracker::reInitModel( - const std::map *> &mapOfImages, - const std::map &mapOfModelFiles, - const std::map &mapOfCameraPoses, - const bool verbose) -{ - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_model = - mapOfModelFiles.find(m_referenceCameraName); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(m_referenceCameraName); - - if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && - it_model != mapOfModelFiles.end() && +void vpMbGenericTracker::reInitModel(const std::map *> &mapOfImages, + const std::map &mapOfModelFiles, + const std::map &mapOfCameraPoses, + const bool verbose) +{ + std::map::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_model = mapOfModelFiles.find(m_referenceCameraName); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName); + + if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) { TrackerWrapper *tracker = it_tracker->second; - tracker->reInitModel(*it_img->second, it_model->second, - it_camPose->second, verbose); + tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose); // Set reference pose tracker->getPose(cMo); } else { - throw vpException(vpTrackingException::fatalError, - "Cannot reInitModel() for reference camera!"); + throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() for reference camera!"); } std::vector vectorOfMissingCameras; - for (it_tracker = m_mapOfTrackers.begin(); - it_tracker != m_mapOfTrackers.end(); ++it_tracker) { + for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) { if (it_tracker->first != m_referenceCameraName) { it_img = mapOfImages.find(it_tracker->first); it_model = mapOfModelFiles.find(it_tracker->first); it_camPose = mapOfCameraPoses.find(it_tracker->first); - if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && - it_camPose != mapOfCameraPoses.end()) { + if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) { TrackerWrapper *tracker = it_tracker->second; - tracker->reInitModel(*it_img->second, it_model->second, - it_camPose->second, verbose); + tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose); } else { vectorOfMissingCameras.push_back(it_tracker->first); } } } - for (std::vector::const_iterator it = - vectorOfMissingCameras.begin(); - it != vectorOfMissingCameras.end(); ++it) { + for (std::vector::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end(); + ++it) { it_img = mapOfImages.find(*it); it_model = mapOfModelFiles.find(*it); std::map::const_iterator it_camTrans = @@ -2625,8 +2316,7 @@ void vpMbGenericTracker::reInitModel( if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; - m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, - cCurrentMo, verbose); + m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose); } } @@ -2670,16 +2360,14 @@ void vpMbGenericTracker::resetTracker() // Reset default ponderation between each feature type m_mapOfFeatureFactors[EDGE_TRACKER] = 1.0; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) m_mapOfFeatureFactors[KLT_TRACKER] = 1.0; #endif m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0; m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0; - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->resetTracker(); @@ -2699,8 +2387,7 @@ void vpMbGenericTracker::setAngleAppear(const double &a) { vpMbTracker::setAngleAppear(a); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setAngleAppear(a); @@ -2722,8 +2409,7 @@ void vpMbGenericTracker::setAngleAppear(const double &a) void vpMbGenericTracker::setAngleAppear(const double &a1, const double &a2) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->setAngleAppear(a1); ++it; @@ -2733,12 +2419,10 @@ void vpMbGenericTracker::setAngleAppear(const double &a1, const double &a2) if (it != m_mapOfTrackers.end()) { angleAppears = it->second->getAngleAppear(); } else { - std::cerr << "Cannot find the reference camera: " - << m_referenceCameraName << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } } else { - throw vpException(vpTrackingException::fatalError, - "Require two cameras! There are %d cameras!", + throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!", m_mapOfTrackers.size()); } } @@ -2752,13 +2436,10 @@ void vpMbGenericTracker::setAngleAppear(const double &a1, const double &a2) \param mapOfAngles : Map of new angles in radian. */ -void vpMbGenericTracker::setAngleAppear( - const std::map &mapOfAngles) +void vpMbGenericTracker::setAngleAppear(const std::map &mapOfAngles) { - for (std::map::const_iterator it = mapOfAngles.begin(); - it != mapOfAngles.end(); ++it) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(it->first); + for (std::map::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) { + std::map::const_iterator it_tracker = m_mapOfTrackers.find(it->first); if (it_tracker != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it_tracker->second; @@ -2784,8 +2465,7 @@ void vpMbGenericTracker::setAngleDisappear(const double &a) { vpMbTracker::setAngleDisappear(a); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setAngleDisappear(a); @@ -2807,8 +2487,7 @@ void vpMbGenericTracker::setAngleDisappear(const double &a) void vpMbGenericTracker::setAngleDisappear(const double &a1, const double &a2) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->setAngleDisappear(a1); ++it; @@ -2818,12 +2497,10 @@ void vpMbGenericTracker::setAngleDisappear(const double &a1, const double &a2) if (it != m_mapOfTrackers.end()) { angleDisappears = it->second->getAngleDisappear(); } else { - std::cerr << "Cannot find the reference camera: " - << m_referenceCameraName << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } } else { - throw vpException(vpTrackingException::fatalError, - "Require two cameras! There are %d cameras!", + throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!", m_mapOfTrackers.size()); } } @@ -2837,13 +2514,10 @@ void vpMbGenericTracker::setAngleDisappear(const double &a1, const double &a2) \param mapOfAngles : Map of new angles in radian. */ -void vpMbGenericTracker::setAngleDisappear( - const std::map &mapOfAngles) +void vpMbGenericTracker::setAngleDisappear(const std::map &mapOfAngles) { - for (std::map::const_iterator it = mapOfAngles.begin(); - it != mapOfAngles.end(); ++it) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(it->first); + for (std::map::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) { + std::map::const_iterator it_tracker = m_mapOfTrackers.find(it->first); if (it_tracker != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it_tracker->second; @@ -2865,8 +2539,7 @@ void vpMbGenericTracker::setCameraParameters(const vpCameraParameters &camera) { vpMbTracker::setCameraParameters(camera); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setCameraParameters(camera); @@ -2881,12 +2554,10 @@ void vpMbGenericTracker::setCameraParameters(const vpCameraParameters &camera) \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::setCameraParameters( - const vpCameraParameters &camera1, const vpCameraParameters &camera2) +void vpMbGenericTracker::setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->setCameraParameters(camera1); ++it; @@ -2896,12 +2567,10 @@ void vpMbGenericTracker::setCameraParameters( if (it != m_mapOfTrackers.end()) { it->second->getCameraParameters(cam); } else { - std::cerr << "Cannot find the reference camera: " - << m_referenceCameraName << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } } else { - throw vpException(vpTrackingException::fatalError, - "Require two cameras! There are %d cameras!", + throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!", m_mapOfTrackers.size()); } } @@ -2914,14 +2583,11 @@ void vpMbGenericTracker::setCameraParameters( \note This function will set the camera parameters only for the supplied camera names. */ -void vpMbGenericTracker::setCameraParameters( - const std::map &mapOfCameraParameters) +void vpMbGenericTracker::setCameraParameters(const std::map &mapOfCameraParameters) { - for (std::map::const_iterator it = - mapOfCameraParameters.begin(); + for (std::map::const_iterator it = mapOfCameraParameters.begin(); it != mapOfCameraParameters.end(); ++it) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(it->first); + std::map::const_iterator it_tracker = m_mapOfTrackers.find(it->first); if (it_tracker != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it_tracker->second; @@ -2942,18 +2608,15 @@ void vpMbGenericTracker::setCameraParameters( \param cameraTransformationMatrix : Camera transformation matrix between the current and the reference camera. */ -void vpMbGenericTracker::setCameraTransformationMatrix( - const std::string &cameraName, - const vpHomogeneousMatrix &cameraTransformationMatrix) +void vpMbGenericTracker::setCameraTransformationMatrix(const std::string &cameraName, + const vpHomogeneousMatrix &cameraTransformationMatrix) { - std::map::iterator it = - m_mapOfCameraTransformationMatrix.find(cameraName); + std::map::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName); if (it != m_mapOfCameraTransformationMatrix.end()) { it->second = cameraTransformationMatrix; } else { - throw vpException(vpTrackingException::fatalError, - "Cannot find camera: %s!", cameraName.c_str()); + throw vpException(vpTrackingException::fatalError, "Cannot find camera: %s!", cameraName.c_str()); } } @@ -2965,11 +2628,9 @@ void vpMbGenericTracker::setCameraTransformationMatrix( \param mapOfTransformationMatrix : map of camera transformation matrices. */ void vpMbGenericTracker::setCameraTransformationMatrix( - const std::map - &mapOfTransformationMatrix) + const std::map &mapOfTransformationMatrix) { - for (std::map::const_iterator it = - mapOfTransformationMatrix.begin(); + for (std::map::const_iterator it = mapOfTransformationMatrix.begin(); it != mapOfTransformationMatrix.end(); ++it) { std::map::iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(it->first); @@ -2993,8 +2654,7 @@ void vpMbGenericTracker::setClipping(const unsigned int &flags) { vpMbTracker::setClipping(flags); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setClipping(flags); @@ -3011,12 +2671,10 @@ void vpMbGenericTracker::setClipping(const unsigned int &flags) \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::setClipping(const unsigned int &flags1, - const unsigned int &flags2) +void vpMbGenericTracker::setClipping(const unsigned int &flags1, const unsigned int &flags2) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->setClipping(flags1); ++it; @@ -3026,13 +2684,11 @@ void vpMbGenericTracker::setClipping(const unsigned int &flags1, if (it != m_mapOfTrackers.end()) { clippingFlag = it->second->getClipping(); } else { - std::cerr << "Cannot find the reference camera: " - << m_referenceCameraName << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } } else { std::stringstream ss; - ss << "Require two cameras! There are " << m_mapOfTrackers.size() - << " cameras!"; + ss << "Require two cameras! There are " << m_mapOfTrackers.size() << " cameras!"; throw vpException(vpTrackingException::fatalError, ss.str()); } } @@ -3046,11 +2702,9 @@ void vpMbGenericTracker::setClipping(const unsigned int &flags1, \sa setDepthDenseFilteringMethod \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setDepthDenseFilteringMaxDistance( - const double maxDistance) +void vpMbGenericTracker::setDepthDenseFilteringMaxDistance(const double maxDistance) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setDepthDenseFilteringMaxDistance(maxDistance); @@ -3067,8 +2721,7 @@ void vpMbGenericTracker::setDepthDenseFilteringMaxDistance( */ void vpMbGenericTracker::setDepthDenseFilteringMethod(const int method) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setDepthDenseFilteringMethod(method); @@ -3084,11 +2737,9 @@ void vpMbGenericTracker::setDepthDenseFilteringMethod(const int method) \sa setDepthDenseFilteringMethod \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setDepthDenseFilteringMinDistance( - const double minDistance) +void vpMbGenericTracker::setDepthDenseFilteringMinDistance(const double minDistance) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setDepthDenseFilteringMinDistance(minDistance); @@ -3104,11 +2755,9 @@ void vpMbGenericTracker::setDepthDenseFilteringMinDistance( \sa setDepthDenseFilteringMethod \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setDepthDenseFilteringOccupancyRatio( - const double occupancyRatio) +void vpMbGenericTracker::setDepthDenseFilteringOccupancyRatio(const double occupancyRatio) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setDepthDenseFilteringOccupancyRatio(occupancyRatio); @@ -3123,11 +2772,9 @@ void vpMbGenericTracker::setDepthDenseFilteringOccupancyRatio( \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setDepthDenseSamplingStep(const unsigned int stepX, - const unsigned int stepY) +void vpMbGenericTracker::setDepthDenseSamplingStep(const unsigned int stepX, const unsigned int stepY) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setDepthDenseSamplingStep(stepX, stepY); @@ -3141,11 +2788,9 @@ void vpMbGenericTracker::setDepthDenseSamplingStep(const unsigned int stepX, \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setDepthNormalFaceCentroidMethod( - const vpMbtFaceDepthNormal::vpFaceCentroidType &method) +void vpMbGenericTracker::setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setDepthNormalFaceCentroidMethod(method); @@ -3162,8 +2807,7 @@ void vpMbGenericTracker::setDepthNormalFaceCentroidMethod( void vpMbGenericTracker::setDepthNormalFeatureEstimationMethod( const vpMbtFaceDepthNormal::vpFeatureEstimationType &method) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setDepthNormalFeatureEstimationMethod(method); @@ -3177,11 +2821,9 @@ void vpMbGenericTracker::setDepthNormalFeatureEstimationMethod( \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setDepthNormalPclPlaneEstimationMethod( - const int method) +void vpMbGenericTracker::setDepthNormalPclPlaneEstimationMethod(const int method) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setDepthNormalPclPlaneEstimationMethod(method); @@ -3195,11 +2837,9 @@ void vpMbGenericTracker::setDepthNormalPclPlaneEstimationMethod( \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setDepthNormalPclPlaneEstimationRansacMaxIter( - const int maxIter) +void vpMbGenericTracker::setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setDepthNormalPclPlaneEstimationRansacMaxIter(maxIter); @@ -3213,11 +2853,9 @@ void vpMbGenericTracker::setDepthNormalPclPlaneEstimationRansacMaxIter( \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setDepthNormalPclPlaneEstimationRansacThreshold( - const double thresold) +void vpMbGenericTracker::setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setDepthNormalPclPlaneEstimationRansacThreshold(thresold); @@ -3232,11 +2870,9 @@ void vpMbGenericTracker::setDepthNormalPclPlaneEstimationRansacThreshold( \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setDepthNormalSamplingStep(const unsigned int stepX, - const unsigned int stepY) +void vpMbGenericTracker::setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setDepthNormalSamplingStep(stepX, stepY); @@ -3250,14 +2886,11 @@ void vpMbGenericTracker::setDepthNormalSamplingStep(const unsigned int stepX, \param mapOfClippingFlags : Map of new clipping flags. */ -void vpMbGenericTracker::setClipping( - const std::map &mapOfClippingFlags) +void vpMbGenericTracker::setClipping(const std::map &mapOfClippingFlags) { - for (std::map::const_iterator it = - mapOfClippingFlags.begin(); + for (std::map::const_iterator it = mapOfClippingFlags.begin(); it != mapOfClippingFlags.end(); ++it) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(it->first); + std::map::const_iterator it_tracker = m_mapOfTrackers.find(it->first); if (it_tracker != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it_tracker->second; @@ -3292,8 +2925,7 @@ void vpMbGenericTracker::setDisplayFeatures(const bool displayF) { vpMbTracker::setDisplayFeatures(displayF); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setDisplayFeatures(displayF); @@ -3311,8 +2943,7 @@ void vpMbGenericTracker::setFarClippingDistance(const double &dist) { vpMbTracker::setFarClippingDistance(dist); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setFarClippingDistance(dist); @@ -3327,12 +2958,10 @@ void vpMbGenericTracker::setFarClippingDistance(const double &dist) \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::setFarClippingDistance(const double &dist1, - const double &dist2) +void vpMbGenericTracker::setFarClippingDistance(const double &dist1, const double &dist2) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->setFarClippingDistance(dist1); ++it; @@ -3342,12 +2971,10 @@ void vpMbGenericTracker::setFarClippingDistance(const double &dist1, if (it != m_mapOfTrackers.end()) { distFarClip = it->second->getFarClippingDistance(); } else { - std::cerr << "Cannot find the reference camera: " - << m_referenceCameraName << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } } else { - throw vpException(vpTrackingException::fatalError, - "Require two cameras! There are %d cameras!", + throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!", m_mapOfTrackers.size()); } } @@ -3357,14 +2984,11 @@ void vpMbGenericTracker::setFarClippingDistance(const double &dist1, \param mapOfClippingDists : Map of far clipping values. */ -void vpMbGenericTracker::setFarClippingDistance( - const std::map &mapOfClippingDists) +void vpMbGenericTracker::setFarClippingDistance(const std::map &mapOfClippingDists) { - for (std::map::const_iterator it = - mapOfClippingDists.begin(); - it != mapOfClippingDists.end(); ++it) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(it->first); + for (std::map::const_iterator it = mapOfClippingDists.begin(); it != mapOfClippingDists.end(); + ++it) { + std::map::const_iterator it_tracker = m_mapOfTrackers.find(it->first); if (it_tracker != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it_tracker->second; @@ -3383,14 +3007,11 @@ void vpMbGenericTracker::setFarClippingDistance( \param mapOfFeatureFactors : Map of feature factors. */ -void vpMbGenericTracker::setFeatureFactors( - const std::map &mapOfFeatureFactors) -{ - for (std::map::iterator it = - m_mapOfFeatureFactors.begin(); - it != m_mapOfFeatureFactors.end(); ++it) { - std::map::const_iterator it_factor = - mapOfFeatureFactors.find(it->first); +void vpMbGenericTracker::setFeatureFactors(const std::map &mapOfFeatureFactors) +{ + for (std::map::iterator it = m_mapOfFeatureFactors.begin(); it != m_mapOfFeatureFactors.end(); + ++it) { + std::map::const_iterator it_factor = mapOfFeatureFactors.find(it->first); if (it_factor != mapOfFeatureFactors.end()) { it->second = it_factor->second; } @@ -3412,13 +3033,11 @@ void vpMbGenericTracker::setFeatureFactors( \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setGoodMovingEdgesRatioThreshold( - const double threshold) +void vpMbGenericTracker::setGoodMovingEdgesRatioThreshold(const double threshold) { m_percentageGdPt = threshold; - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setGoodMovingEdgesRatioThreshold(threshold); @@ -3439,8 +3058,7 @@ void vpMbGenericTracker::setGoodMovingEdgesRatioThreshold( */ void vpMbGenericTracker::setGoodNbRayCastingAttemptsRatio(const double &ratio) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setGoodNbRayCastingAttemptsRatio(ratio); @@ -3458,11 +3076,9 @@ void vpMbGenericTracker::setGoodNbRayCastingAttemptsRatio(const double &ratio) \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setNbRayCastingAttemptsForVisibility( - const unsigned int &attempts) +void vpMbGenericTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setNbRayCastingAttemptsForVisibility(attempts); @@ -3470,8 +3086,7 @@ void vpMbGenericTracker::setNbRayCastingAttemptsForVisibility( } #endif -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) /*! Set the new value of the klt tracker. @@ -3481,8 +3096,7 @@ void vpMbGenericTracker::setNbRayCastingAttemptsForVisibility( */ void vpMbGenericTracker::setKltOpencv(const vpKltOpencv &t) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setKltOpencv(t); @@ -3497,19 +3111,16 @@ void vpMbGenericTracker::setKltOpencv(const vpKltOpencv &t) \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::setKltOpencv(const vpKltOpencv &t1, - const vpKltOpencv &t2) +void vpMbGenericTracker::setKltOpencv(const vpKltOpencv &t1, const vpKltOpencv &t2) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->setKltOpencv(t1); ++it; it->second->setKltOpencv(t2); } else { - throw vpException(vpTrackingException::fatalError, - "Require two cameras! There are %d cameras!", + throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!", m_mapOfTrackers.size()); } } @@ -3519,14 +3130,10 @@ void vpMbGenericTracker::setKltOpencv(const vpKltOpencv &t1, \param mapOfKlts : Map of klt tracker containing the new values. */ -void vpMbGenericTracker::setKltOpencv( - const std::map &mapOfKlts) +void vpMbGenericTracker::setKltOpencv(const std::map &mapOfKlts) { - for (std::map::const_iterator it = - mapOfKlts.begin(); - it != mapOfKlts.end(); ++it) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(it->first); + for (std::map::const_iterator it = mapOfKlts.begin(); it != mapOfKlts.end(); ++it) { + std::map::const_iterator it_tracker = m_mapOfTrackers.find(it->first); if (it_tracker != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it_tracker->second; @@ -3546,8 +3153,7 @@ void vpMbGenericTracker::setKltThresholdAcceptation(const double th) { m_thresholdOutlier = th; - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setKltThresholdAcceptation(th); @@ -3569,16 +3175,14 @@ void vpMbGenericTracker::setKltThresholdAcceptation(const double th) */ void vpMbGenericTracker::setLod(const bool useLod, const std::string &name) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setLod(useLod, name); } } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) /*! Set the erosion of the mask used on the Model faces. @@ -3588,8 +3192,7 @@ void vpMbGenericTracker::setLod(const bool useLod, const std::string &name) */ void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setKltMaskBorder(e); @@ -3604,20 +3207,17 @@ void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e) \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e1, - const unsigned int &e2) +void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e1, const unsigned int &e2) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->setKltMaskBorder(e1); ++it; it->second->setKltMaskBorder(e2); } else { - throw vpException(vpTrackingException::fatalError, - "Require two cameras! There are %d cameras!", + throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!", m_mapOfTrackers.size()); } } @@ -3627,14 +3227,11 @@ void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e1, \param mapOfErosions : Map of desired erosions. */ -void vpMbGenericTracker::setKltMaskBorder( - const std::map &mapOfErosions) +void vpMbGenericTracker::setKltMaskBorder(const std::map &mapOfErosions) { - for (std::map::const_iterator it = - mapOfErosions.begin(); - it != mapOfErosions.end(); ++it) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(it->first); + for (std::map::const_iterator it = mapOfErosions.begin(); it != mapOfErosions.end(); + ++it) { + std::map::const_iterator it_tracker = m_mapOfTrackers.find(it->first); if (it_tracker != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it_tracker->second; @@ -3655,11 +3252,9 @@ void vpMbGenericTracker::setKltMaskBorder( \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setMinLineLengthThresh( - const double minLineLengthThresh, const std::string &name) +void vpMbGenericTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setMinLineLengthThresh(minLineLengthThresh, name); @@ -3676,11 +3271,9 @@ void vpMbGenericTracker::setMinLineLengthThresh( \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setMinPolygonAreaThresh( - const double minPolygonAreaThresh, const std::string &name) +void vpMbGenericTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setMinPolygonAreaThresh(minPolygonAreaThresh, name); @@ -3696,8 +3289,7 @@ void vpMbGenericTracker::setMinPolygonAreaThresh( */ void vpMbGenericTracker::setMovingEdge(const vpMe &me) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setMovingEdge(me); @@ -3716,16 +3308,14 @@ void vpMbGenericTracker::setMovingEdge(const vpMe &me) void vpMbGenericTracker::setMovingEdge(const vpMe &me1, const vpMe &me2) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->setMovingEdge(me1); ++it; it->second->setMovingEdge(me2); } else { - throw vpException(vpTrackingException::fatalError, - "Require two cameras! There are %d cameras!", + throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!", m_mapOfTrackers.size()); } } @@ -3735,13 +3325,10 @@ void vpMbGenericTracker::setMovingEdge(const vpMe &me1, const vpMe &me2) \param mapOfMe : Map of vpMe containing all the desired parameters. */ -void vpMbGenericTracker::setMovingEdge( - const std::map &mapOfMe) +void vpMbGenericTracker::setMovingEdge(const std::map &mapOfMe) { - for (std::map::const_iterator it = mapOfMe.begin(); - it != mapOfMe.end(); ++it) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(it->first); + for (std::map::const_iterator it = mapOfMe.begin(); it != mapOfMe.end(); ++it) { + std::map::const_iterator it_tracker = m_mapOfTrackers.find(it->first); if (it_tracker != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it_tracker->second; @@ -3761,8 +3348,7 @@ void vpMbGenericTracker::setNearClippingDistance(const double &dist) { vpMbTracker::setNearClippingDistance(dist); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setNearClippingDistance(dist); @@ -3777,12 +3363,10 @@ void vpMbGenericTracker::setNearClippingDistance(const double &dist) \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::setNearClippingDistance(const double &dist1, - const double &dist2) +void vpMbGenericTracker::setNearClippingDistance(const double &dist1, const double &dist2) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->setNearClippingDistance(dist1); ++it; @@ -3793,12 +3377,10 @@ void vpMbGenericTracker::setNearClippingDistance(const double &dist1, if (it != m_mapOfTrackers.end()) { distNearClip = it->second->getNearClippingDistance(); } else { - std::cerr << "Cannot find the reference camera: " - << m_referenceCameraName << "!" << std::endl; + std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl; } } else { - throw vpException(vpTrackingException::fatalError, - "Require two cameras! There are %d cameras!", + throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!", m_mapOfTrackers.size()); } } @@ -3808,13 +3390,10 @@ void vpMbGenericTracker::setNearClippingDistance(const double &dist1, \param mapOfDists : Map of near clipping values. */ -void vpMbGenericTracker::setNearClippingDistance( - const std::map &mapOfDists) +void vpMbGenericTracker::setNearClippingDistance(const std::map &mapOfDists) { - for (std::map::const_iterator it = mapOfDists.begin(); - it != mapOfDists.end(); ++it) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(it->first); + for (std::map::const_iterator it = mapOfDists.begin(); it != mapOfDists.end(); ++it) { + std::map::const_iterator it_tracker = m_mapOfTrackers.find(it->first); if (it_tracker != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it_tracker->second; @@ -3843,8 +3422,7 @@ void vpMbGenericTracker::setOgreShowConfigDialog(const bool showConfigDialog) { vpMbTracker::setOgreShowConfigDialog(showConfigDialog); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setOgreShowConfigDialog(showConfigDialog); @@ -3865,20 +3443,17 @@ void vpMbGenericTracker::setOgreVisibilityTest(const bool &v) { vpMbTracker::setOgreVisibilityTest(v); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setOgreVisibilityTest(v); } #ifdef VISP_HAVE_OGRE - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; - tracker->faces.getOgreContext()->setWindowName("Multi Generic MBT (" + - it->first + ")"); + tracker->faces.getOgreContext()->setWindowName("Multi Generic MBT (" + it->first + ")"); } #endif } @@ -3890,13 +3465,11 @@ void vpMbGenericTracker::setOgreVisibilityTest(const bool &v) \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setOptimizationMethod( - const vpMbtOptimizationMethod &opt) +void vpMbGenericTracker::setOptimizationMethod(const vpMbtOptimizationMethod &opt) { vpMbTracker::setOptimizationMethod(opt); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setOptimizationMethod(opt); @@ -3915,25 +3488,21 @@ void vpMbGenericTracker::setOptimizationMethod( \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setPose(const vpImage &I, - const vpHomogeneousMatrix &cdMo) +void vpMbGenericTracker::setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo) { if (m_mapOfTrackers.size() > 1) { - throw vpException(vpTrackingException::initializationError, - "The function setPose() requires the generic tracker " - "to be configured with only one camera!"); + throw vpException(vpTrackingException::initializationError, "The function setPose() requires the generic tracker " + "to be configured with only one camera!"); } cMo = cdMo; - std::map::const_iterator it = - m_mapOfTrackers.find(m_referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName); if (it != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it->second; tracker->setPose(I, cdMo); } else { - throw vpException(vpTrackingException::initializationError, - "The reference camera: %s does not exist!", + throw vpException(vpTrackingException::initializationError, "The reference camera: %s does not exist!", m_referenceCameraName.c_str()); } } @@ -3949,14 +3518,11 @@ void vpMbGenericTracker::setPose(const vpImage &I, \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::setPose(const vpImage &I1, - const vpImage &I2, - const vpHomogeneousMatrix &c1Mo, - const vpHomogeneousMatrix &c2Mo) +void vpMbGenericTracker::setPose(const vpImage &I1, const vpImage &I2, + const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); it->second->setPose(I1, c1Mo); ++it; @@ -3968,13 +3534,11 @@ void vpMbGenericTracker::setPose(const vpImage &I1, // Set reference pose it->second->getPose(cMo); } else { - throw vpException(vpTrackingException::fatalError, - "The reference camera: %s does not exist!", + throw vpException(vpTrackingException::fatalError, "The reference camera: %s does not exist!", m_referenceCameraName.c_str()); } } else { - throw vpException(vpTrackingException::fatalError, - "Require two cameras! There are %d cameras!", + throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!", m_mapOfTrackers.size()); } } @@ -3994,40 +3558,33 @@ void vpMbGenericTracker::setPose(const vpImage &I1, the known geometric transformation between each camera (see setCameraTransformationMatrix()). */ -void vpMbGenericTracker::setPose( - const std::map *> &mapOfImages, - const std::map &mapOfCameraPoses) +void vpMbGenericTracker::setPose(const std::map *> &mapOfImages, + const std::map &mapOfCameraPoses) { // Set the reference cMo - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(m_referenceCameraName); - std::map *>::const_iterator - it_img = mapOfImages.find(m_referenceCameraName); - std::map::const_iterator it_camPose = - mapOfCameraPoses.find(m_referenceCameraName); - - if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && - it_camPose != mapOfCameraPoses.end()) { + std::map::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName); + std::map *>::const_iterator it_img = + mapOfImages.find(m_referenceCameraName); + std::map::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName); + + if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) { TrackerWrapper *tracker = it_tracker->second; tracker->setPose(*it_img->second, it_camPose->second); tracker->getPose(cMo); } else { - throw vpException(vpTrackingException::fatalError, - "Cannot set pose for the reference camera!"); + throw vpException(vpTrackingException::fatalError, "Cannot set pose for the reference camera!"); } // Vector of missing pose matrices for cameras std::vector vectorOfMissingCameraPoses; // Set pose for the specified cameras - for (it_tracker = m_mapOfTrackers.begin(); - it_tracker != m_mapOfTrackers.end(); ++it_tracker) { + for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) { if (it_tracker->first != m_referenceCameraName) { it_img = mapOfImages.find(it_tracker->first); it_camPose = mapOfCameraPoses.find(it_tracker->first); - if (it_img != mapOfImages.end() && - it_camPose != mapOfCameraPoses.end()) { + if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) { // Set pose TrackerWrapper *tracker = it_tracker->second; tracker->setPose(*it_img->second, it_camPose->second); @@ -4037,15 +3594,13 @@ void vpMbGenericTracker::setPose( } } - for (std::vector::const_iterator it = - vectorOfMissingCameraPoses.begin(); + for (std::vector::const_iterator it = vectorOfMissingCameraPoses.begin(); it != vectorOfMissingCameraPoses.end(); ++it) { it_img = mapOfImages.find(*it); std::map::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it); - if (it_img != mapOfImages.end() && - it_camTrans != m_mapOfCameraTransformationMatrix.end()) { + if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) { vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo; m_mapOfTrackers[*it]->setPose(*it_img->second, cCurrentMo); } else { @@ -4075,8 +3630,7 @@ void vpMbGenericTracker::setProjectionErrorComputation(const bool &flag) { vpMbTracker::setProjectionErrorComputation(flag); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setProjectionErrorComputation(flag); @@ -4088,16 +3642,13 @@ void vpMbGenericTracker::setProjectionErrorComputation(const bool &flag) \param referenceCameraName : Name of the reference camera. */ -void vpMbGenericTracker::setReferenceCameraName( - const std::string &referenceCameraName) +void vpMbGenericTracker::setReferenceCameraName(const std::string &referenceCameraName) { - std::map::const_iterator it = - m_mapOfTrackers.find(referenceCameraName); + std::map::const_iterator it = m_mapOfTrackers.find(referenceCameraName); if (it != m_mapOfTrackers.end()) { m_referenceCameraName = referenceCameraName; } else { - std::cerr << "The reference camera: " << referenceCameraName - << " does not exist!"; + std::cerr << "The reference camera: " << referenceCameraName << " does not exist!"; } } @@ -4105,8 +3656,7 @@ void vpMbGenericTracker::setScanLineVisibilityTest(const bool &v) { vpMbTracker::setScanLineVisibilityTest(v); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setScanLineVisibilityTest(v); @@ -4126,8 +3676,7 @@ void vpMbGenericTracker::setScanLineVisibilityTest(const bool &v) */ void vpMbGenericTracker::setTrackerType(const int type) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setTrackerType(type); @@ -4143,14 +3692,10 @@ void vpMbGenericTracker::setTrackerType(const int type) \warning This function has to be called before the loading of the CAD model. */ -void vpMbGenericTracker::setTrackerType( - const std::map &mapOfTrackerTypes) -{ - for (std::map::const_iterator it = - mapOfTrackerTypes.begin(); - it != mapOfTrackerTypes.end(); ++it) { - std::map::const_iterator it_tracker = - m_mapOfTrackers.find(it->first); +void vpMbGenericTracker::setTrackerType(const std::map &mapOfTrackerTypes) +{ + for (std::map::const_iterator it = mapOfTrackerTypes.begin(); it != mapOfTrackerTypes.end(); ++it) { + std::map::const_iterator it_tracker = m_mapOfTrackers.find(it->first); if (it_tracker != m_mapOfTrackers.end()) { TrackerWrapper *tracker = it_tracker->second; tracker->setTrackerType(it->second); @@ -4167,19 +3712,16 @@ void vpMbGenericTracker::setTrackerType( \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setUseEdgeTracking(const std::string &name, - const bool &useEdgeTracking) +void vpMbGenericTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setUseEdgeTracking(name, useEdgeTracking); } } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) /*! Set if the polygons that have the given name have to be considered during the tracking phase. @@ -4189,11 +3731,9 @@ void vpMbGenericTracker::setUseEdgeTracking(const std::string &name, \note This function will set the new parameter for all the cameras. */ -void vpMbGenericTracker::setUseKltTracking(const std::string &name, - const bool &useKltTracking) +void vpMbGenericTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; tracker->setUseKltTracking(name, useKltTracking); @@ -4205,8 +3745,7 @@ void vpMbGenericTracker::testTracking() { // Test tracking fails only if all testTracking have failed bool isOneTestTrackingOk = false; - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; try { @@ -4255,12 +3794,10 @@ void vpMbGenericTracker::track(const vpImage &I) \note This function assumes a stereo configuration of the generic tracker. */ -void vpMbGenericTracker::track(const vpImage &I1, - const vpImage &I2) +void vpMbGenericTracker::track(const vpImage &I1, const vpImage &I2) { if (m_mapOfTrackers.size() == 2) { - std::map::const_iterator it = - m_mapOfTrackers.begin(); + std::map::const_iterator it = m_mapOfTrackers.begin(); std::map *> mapOfImages; mapOfImages[it->first] = &I1; ++it; @@ -4273,8 +3810,7 @@ void vpMbGenericTracker::track(const vpImage &I1, track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights); } else { std::stringstream ss; - ss << "Require two cameras! There are " << m_mapOfTrackers.size() - << " cameras!"; + ss << "Require two cameras! There are " << m_mapOfTrackers.size() << " cameras!"; throw vpException(vpTrackingException::fatalError, ss.str().c_str()); } } @@ -4286,8 +3822,7 @@ void vpMbGenericTracker::track(const vpImage &I1, \param mapOfImages : Map of images. */ -void vpMbGenericTracker::track( - std::map *> &mapOfImages) +void vpMbGenericTracker::track(std::map *> &mapOfImages) { std::map *> mapOfPointClouds; std::map mapOfWidths, mapOfHeights; @@ -4304,30 +3839,23 @@ void vpMbGenericTracker::track( \param mapOfImages : Map of images. \param mapOfPointClouds : Map of PCL pointclouds. */ -void vpMbGenericTracker::track( - std::map *> &mapOfImages, - std::map::ConstPtr> - &mapOfPointClouds) +void vpMbGenericTracker::track(std::map *> &mapOfImages, + std::map::ConstPtr> &mapOfPointClouds) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; - if ((tracker->m_trackerType & - (EDGE_TRACKER | -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) - KLT_TRACKER | + if ((tracker->m_trackerType & (EDGE_TRACKER | +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) + KLT_TRACKER | #endif - DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) { - throw vpException(vpException::fatalError, "Bad tracker type: %d", - tracker->m_trackerType); + DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) { + throw vpException(vpException::fatalError, "Bad tracker type: %d", tracker->m_trackerType); } if (tracker->m_trackerType & (EDGE_TRACKER -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) | KLT_TRACKER #endif ) && @@ -4335,11 +3863,9 @@ void vpMbGenericTracker::track( throw vpException(vpException::fatalError, "Image pointer is NULL!"); } - if (tracker->m_trackerType & - (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && + if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && mapOfPointClouds[it->first] == nullptr) { - throw vpException(vpException::fatalError, - "Pointcloud smart pointer is NULL!"); + throw vpException(vpException::fatalError, "Pointcloud smart pointer is NULL!"); } } @@ -4354,13 +3880,11 @@ void vpMbGenericTracker::track( testTracking(); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; - tracker->postTracking(mapOfImages[it->first], - mapOfPointClouds[it->first]); + tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]); } computeProjectionError(); @@ -4377,31 +3901,25 @@ void vpMbGenericTracker::track( \param mapOfPointCloudWidths : Map of pointcloud widths. \param mapOfPointCloudHeights : Map of pointcloud heights. */ -void vpMbGenericTracker::track( - std::map *> &mapOfImages, - std::map *> &mapOfPointClouds, - std::map &mapOfPointCloudWidths, - std::map &mapOfPointCloudHeights) +void vpMbGenericTracker::track(std::map *> &mapOfImages, + std::map *> &mapOfPointClouds, + std::map &mapOfPointCloudWidths, + std::map &mapOfPointCloudHeights) { - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; - if ((tracker->m_trackerType & - (EDGE_TRACKER | -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) - KLT_TRACKER | + if ((tracker->m_trackerType & (EDGE_TRACKER | +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) + KLT_TRACKER | #endif - DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) { - throw vpException(vpException::fatalError, "Bad tracker type: %d", - tracker->m_trackerType); + DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) { + throw vpException(vpException::fatalError, "Bad tracker type: %d", tracker->m_trackerType); } if (tracker->m_trackerType & (EDGE_TRACKER -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) | KLT_TRACKER #endif ) && @@ -4409,15 +3927,13 @@ void vpMbGenericTracker::track( throw vpException(vpException::fatalError, "Image pointer is NULL!"); } - if (tracker->m_trackerType & - (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && + if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && (mapOfPointClouds[it->first] == NULL)) { throw vpException(vpException::fatalError, "Pointcloud is NULL!"); } } - preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, - mapOfPointCloudHeights); + preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights); try { computeVVS(mapOfImages); @@ -4428,14 +3944,11 @@ void vpMbGenericTracker::track( testTracking(); - for (std::map::const_iterator it = - m_mapOfTrackers.begin(); + for (std::map::const_iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end(); ++it) { TrackerWrapper *tracker = it->second; - tracker->postTracking(mapOfImages[it->first], - mapOfPointCloudWidths[it->first], - mapOfPointCloudHeights[it->first]); + tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]); } computeProjectionError(); @@ -4457,13 +3970,11 @@ vpMbGenericTracker::TrackerWrapper::TrackerWrapper(const int trackerType) : m_error(), m_L(), m_trackerType(trackerType), m_w(), m_weightedError() { if ((m_trackerType & (EDGE_TRACKER | -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) KLT_TRACKER | #endif DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) { - throw vpException(vpTrackingException::badValue, - "Bad value for tracker type: %d!", m_trackerType); + throw vpException(vpTrackingException::badValue, "Bad value for tracker type: %d!", m_trackerType); } m_lambda = 1.0; @@ -4478,14 +3989,12 @@ vpMbGenericTracker::TrackerWrapper::~TrackerWrapper() {} // Implemented only for debugging purposes: use TrackerWrapper as a standalone // tracker -void vpMbGenericTracker::TrackerWrapper::computeVVS( - const vpImage *const ptr_I) +void vpMbGenericTracker::TrackerWrapper::computeVVS(const vpImage *const ptr_I) { computeVVSInit(ptr_I); if (m_error.getRows() < 4) { - throw vpTrackingException(vpTrackingException::notEnoughPointError, - "Error: not enough features"); + throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features"); } double normRes = 0; @@ -4493,8 +4002,7 @@ void vpMbGenericTracker::TrackerWrapper::computeVVS( unsigned int iter = 0; double factorEdge = 1.0; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) double factorKlt = 1.0; #endif double factorDepth = 1.0; @@ -4506,8 +4014,7 @@ void vpMbGenericTracker::TrackerWrapper::computeVVS( double mu = m_initialMu; vpHomogeneousMatrix cMo_prev; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) vpHomogeneousMatrix ctTc0_Prev; // Only for KLT #endif bool isoJoIdentity_ = true; @@ -4517,23 +4024,19 @@ void vpMbGenericTracker::TrackerWrapper::computeVVS( vpMatrix L_true, LVJ_true; unsigned int nb_edge_features = m_error_edge.getRows(); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) unsigned int nb_klt_features = m_error_klt.getRows(); #endif unsigned int nb_depth_features = m_error_depthNormal.getRows(); unsigned int nb_depth_dense_features = m_error_depthDense.getRows(); - while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && - (iter < m_maxIter)) { + while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) { computeVVSInteractionMatrixAndResidu(ptr_I); bool reStartFromLastIncrement = false; - computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, - reStartFromLastIncrement); + computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (reStartFromLastIncrement) { if (m_trackerType & KLT_TRACKER) { ctTc0 = ctTc0_Prev; @@ -4568,8 +4071,7 @@ void vpMbGenericTracker::TrackerWrapper::computeVVS( vpMatrix K; // kernel unsigned int rank = (m_L * cVo).kernel(K); if (rank == 0) { - throw vpException(vpException::fatalError, - "Rank=0, cannot estimate the pose !"); + throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !"); } if (rank != 6) { @@ -4604,8 +4106,7 @@ void vpMbGenericTracker::TrackerWrapper::computeVVS( start_index += nb_edge_features; } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) { for (unsigned int i = 0; i < nb_klt_features; i++) { double wi = m_w_klt[i] * factorKlt; @@ -4658,13 +4159,10 @@ void vpMbGenericTracker::TrackerWrapper::computeVVS( // start_index += nb_depth_dense_features; } - computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, - m_weightedError, m_error, error_prev, LTR, mu, - v); + computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, m_weightedError, m_error, error_prev, LTR, mu, v); cMo_prev = cMo; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) { ctTc0_Prev = ctTc0; } @@ -4672,8 +4170,7 @@ void vpMbGenericTracker::TrackerWrapper::computeVVS( cMo = vpExponentialMap::direct(v).inverse() * cMo; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) { ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0; } @@ -4686,8 +4183,7 @@ void vpMbGenericTracker::TrackerWrapper::computeVVS( iter++; } - computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, - LVJ_true, m_error); + computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error); if (m_trackerType & EDGE_TRACKER) { vpMbEdgeTracker::updateMovingEdgeWeights(); @@ -4701,8 +4197,7 @@ void vpMbGenericTracker::TrackerWrapper::computeVVSInit() ") should not be called!"); } -void vpMbGenericTracker::TrackerWrapper::computeVVSInit( - const vpImage *const ptr_I) +void vpMbGenericTracker::TrackerWrapper::computeVVSInit(const vpImage *const ptr_I) { initMbtTracking(ptr_I); @@ -4717,8 +4212,7 @@ void vpMbGenericTracker::TrackerWrapper::computeVVSInit( m_w_edge.clear(); } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) { vpMbKltTracker::computeVVSInit(); nbFeatures += m_error_klt.getRows(); @@ -4758,8 +4252,7 @@ void vpMbGenericTracker::TrackerWrapper::computeVVSInit( m_w = 1; } -void vpMbGenericTracker::TrackerWrapper:: - computeVVSInteractionMatrixAndResidu() +void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu() { throw vpException(vpException::fatalError, "vpMbGenericTracker::" "TrackerWrapper::" @@ -4767,15 +4260,13 @@ void vpMbGenericTracker::TrackerWrapper:: "esidu() should not be called!"); } -void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu( - const vpImage *const ptr_I) +void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu(const vpImage *const ptr_I) { if (m_trackerType & EDGE_TRACKER) { vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu(*ptr_I); } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) { vpMbKltTracker::computeVVSInteractionMatrixAndResidu(); } @@ -4797,8 +4288,7 @@ void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu( start_index += m_error_edge.getRows(); } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) { m_L.insert(m_L_klt, start_index, 0); m_error.insert(start_index, m_error_klt); @@ -4833,8 +4323,7 @@ void vpMbGenericTracker::TrackerWrapper::computeVVSWeights() start_index += m_w_edge.getRows(); } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) { vpMbTracker::computeVVSWeights(m_robust_klt, m_error_klt, m_w_klt); m_w.insert(start_index, m_w_klt); @@ -4845,8 +4334,7 @@ void vpMbGenericTracker::TrackerWrapper::computeVVSWeights() if (m_trackerType & DEPTH_NORMAL_TRACKER) { if (m_depthNormalUseRobust) { - vpMbTracker::computeVVSWeights(m_robust_depthNormal, - m_error_depthNormal, m_w_depthNormal); + vpMbTracker::computeVVSWeights(m_robust_depthNormal, m_error_depthNormal, m_w_depthNormal); m_w.insert(start_index, m_w_depthNormal); } @@ -4861,44 +4349,35 @@ void vpMbGenericTracker::TrackerWrapper::computeVVSWeights() } } -void vpMbGenericTracker::TrackerWrapper::display( - const vpImage &I, const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &camera, const vpColor &col, - const unsigned int thickness, const bool displayFullModel) +void vpMbGenericTracker::TrackerWrapper::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &camera, const vpColor &col, + const unsigned int thickness, const bool displayFullModel) { if (m_trackerType == EDGE_TRACKER) { - vpMbEdgeTracker::display(I, cMo_, camera, col, thickness, - displayFullModel); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) + vpMbEdgeTracker::display(I, cMo_, camera, col, thickness, displayFullModel); +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) } else if (m_trackerType == KLT_TRACKER) { - vpMbKltTracker::display(I, cMo_, camera, col, thickness, - displayFullModel); + vpMbKltTracker::display(I, cMo_, camera, col, thickness, displayFullModel); #endif } else if (m_trackerType == DEPTH_NORMAL_TRACKER) { - vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, - displayFullModel); + vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel); } else if (m_trackerType == DEPTH_DENSE_TRACKER) { - vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, - displayFullModel); + vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, displayFullModel); } else { if (m_trackerType & EDGE_TRACKER) { for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[scaleLevel].begin(); + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } @@ -4908,38 +4387,31 @@ void vpMbGenericTracker::TrackerWrapper::display( } } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) { - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); + ++it) { vpMbtDistanceKltPoints *kltpoly = *it; - if (displayFeatures && kltpoly->hasEnoughPoints() && - kltpoly->isTracked() && kltpoly->polygon->isVisible()) { + if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) { kltpoly->displayPrimitive(I); } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; - if (displayFeatures && kltPolyCylinder->isTracked() && - kltPolyCylinder->hasEnoughPoints()) + if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) kltPolyCylinder->displayPrimitive(I); } } #endif if (m_trackerType & DEPTH_NORMAL_TRACKER) { - vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, - displayFullModel); + vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel); } if (m_trackerType & DEPTH_DENSE_TRACKER) { - vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, - displayFullModel); + vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, displayFullModel); } #ifdef VISP_HAVE_OGRE @@ -4949,44 +4421,35 @@ void vpMbGenericTracker::TrackerWrapper::display( } } -void vpMbGenericTracker::TrackerWrapper::display( - const vpImage &I, const vpHomogeneousMatrix &cMo_, - const vpCameraParameters &camera, const vpColor &col, - const unsigned int thickness, const bool displayFullModel) +void vpMbGenericTracker::TrackerWrapper::display(const vpImage &I, const vpHomogeneousMatrix &cMo_, + const vpCameraParameters &camera, const vpColor &col, + const unsigned int thickness, const bool displayFullModel) { if (m_trackerType == EDGE_TRACKER) { - vpMbEdgeTracker::display(I, cMo_, camera, col, thickness, - displayFullModel); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) + vpMbEdgeTracker::display(I, cMo_, camera, col, thickness, displayFullModel); +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) } else if (m_trackerType == KLT_TRACKER) { - vpMbKltTracker::display(I, cMo_, camera, col, thickness, - displayFullModel); + vpMbKltTracker::display(I, cMo_, camera, col, thickness, displayFullModel); #endif } else if (m_trackerType == DEPTH_NORMAL_TRACKER) { - vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, - displayFullModel); + vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel); } else if (m_trackerType == DEPTH_DENSE_TRACKER) { - vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, - displayFullModel); + vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, displayFullModel); } else { if (m_trackerType & EDGE_TRACKER) { for (unsigned int i = 0; i < scales.size(); i += 1) { if (scales[i]) { - for (std::list::const_iterator it = - lines[scaleLevel].begin(); + for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } - for (std::list::const_iterator it = - cylinders[scaleLevel].begin(); + for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } - for (std::list::const_iterator it = - circles[scaleLevel].begin(); + for (std::list::const_iterator it = circles[scaleLevel].begin(); it != circles[scaleLevel].end(); ++it) { (*it)->display(I, cMo_, camera, col, thickness, displayFullModel); } @@ -4996,38 +4459,31 @@ void vpMbGenericTracker::TrackerWrapper::display( } } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) { - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); + ++it) { vpMbtDistanceKltPoints *kltpoly = *it; - if (displayFeatures && kltpoly->hasEnoughPoints() && - kltpoly->isTracked() && kltpoly->polygon->isVisible()) { + if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) { kltpoly->displayPrimitive(I); } } - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; - if (displayFeatures && kltPolyCylinder->isTracked() && - kltPolyCylinder->hasEnoughPoints()) + if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) kltPolyCylinder->displayPrimitive(I); } } #endif if (m_trackerType & DEPTH_NORMAL_TRACKER) { - vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, - displayFullModel); + vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel); } if (m_trackerType & DEPTH_DENSE_TRACKER) { - vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, - displayFullModel); + vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel); } #ifdef VISP_HAVE_OGRE @@ -5048,8 +4504,7 @@ void vpMbGenericTracker::TrackerWrapper::init(const vpImage &I) bool reInitialisation = false; if (!useOgre) { - faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); } else { #ifdef VISP_HAVE_OGRE if (!faces.isOgreInitialised()) { @@ -5063,11 +4518,9 @@ void vpMbGenericTracker::TrackerWrapper::init(const vpImage &I) ogreShowConfigDialog = false; } - faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); #else - faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, - reInitialisation); + faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation); #endif } @@ -5076,8 +4529,7 @@ void vpMbGenericTracker::TrackerWrapper::init(const vpImage &I) faces.computeScanLineRender(cam, I.getWidth(), I.getHeight()); } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) vpMbKltTracker::reinit(I); #endif @@ -5086,53 +4538,43 @@ void vpMbGenericTracker::TrackerWrapper::init(const vpImage &I) vpMbEdgeTracker::resetMovingEdge(); bool a = false; - vpMbEdgeTracker::visibleFace( - I, cMo, a); // should be useless, but keep it for nbvisiblepolygone + vpMbEdgeTracker::visibleFace(I, cMo, a); // should be useless, but keep it for nbvisiblepolygone initMovingEdge(I, cMo); } if (m_trackerType & DEPTH_NORMAL_TRACKER) - vpMbDepthNormalTracker::computeVisibility( - I.getWidth(), I.getHeight()); // vpMbDepthNormalTracker::init(I); + vpMbDepthNormalTracker::computeVisibility(I.getWidth(), I.getHeight()); // vpMbDepthNormalTracker::init(I); if (m_trackerType & DEPTH_DENSE_TRACKER) - vpMbDepthDenseTracker::computeVisibility( - I.getWidth(), I.getHeight()); // vpMbDepthDenseTracker::init(I); + vpMbDepthDenseTracker::computeVisibility(I.getWidth(), I.getHeight()); // vpMbDepthDenseTracker::init(I); } -void vpMbGenericTracker::TrackerWrapper::initCircle( - const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, - const double radius, const int idFace, const std::string &name) +void vpMbGenericTracker::TrackerWrapper::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, + const double radius, const int idFace, const std::string &name) { if (m_trackerType & EDGE_TRACKER) vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name); } -void vpMbGenericTracker::TrackerWrapper::initCylinder(const vpPoint &p1, - const vpPoint &p2, - const double radius, - const int idFace, - const std::string &name) +void vpMbGenericTracker::TrackerWrapper::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, + const int idFace, const std::string &name) { if (m_trackerType & EDGE_TRACKER) vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name); #endif } -void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners( - vpMbtPolygon &polygon) +void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners(vpMbtPolygon &polygon) { if (m_trackerType & EDGE_TRACKER) vpMbEdgeTracker::initFaceFromCorners(polygon); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) vpMbKltTracker::initFaceFromCorners(polygon); #endif @@ -5144,14 +4586,12 @@ void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners( vpMbDepthDenseTracker::initFaceFromCorners(polygon); } -void vpMbGenericTracker::TrackerWrapper::initFaceFromLines( - vpMbtPolygon &polygon) +void vpMbGenericTracker::TrackerWrapper::initFaceFromLines(vpMbtPolygon &polygon) { if (m_trackerType & EDGE_TRACKER) vpMbEdgeTracker::initFaceFromLines(polygon); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) vpMbKltTracker::initFaceFromLines(polygon); #endif @@ -5163,8 +4603,7 @@ void vpMbGenericTracker::TrackerWrapper::initFaceFromLines( vpMbDepthDenseTracker::initFaceFromLines(polygon); } -void vpMbGenericTracker::TrackerWrapper::initMbtTracking( - const vpImage *const ptr_I) +void vpMbGenericTracker::TrackerWrapper::initMbtTracking(const vpImage *const ptr_I) { if (m_trackerType & EDGE_TRACKER) { vpMbEdgeTracker::computeVVSInit(); @@ -5172,12 +4611,10 @@ void vpMbGenericTracker::TrackerWrapper::initMbtTracking( } } -void vpMbGenericTracker::TrackerWrapper::loadConfigFile( - const std::string &configFile) +void vpMbGenericTracker::TrackerWrapper::loadConfigFile(const std::string &configFile) { #ifdef VISP_HAVE_XML2 - vpMbtXmlGenericParser xmlp( - (vpMbtXmlGenericParser::vpParserType)m_trackerType); + vpMbtXmlGenericParser xmlp((vpMbtXmlGenericParser::vpParserType)m_trackerType); xmlp.setCameraParameters(cam); xmlp.setAngleAppear(vpMath::deg(angleAppears)); @@ -5194,20 +4631,15 @@ void vpMbGenericTracker::TrackerWrapper::loadConfigFile( xmlp.setKltHarrisParam(0.01); xmlp.setKltBlockSize(3); xmlp.setKltPyramidLevels(3); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) xmlp.setKltMaskBorder(maskBorder); #endif // Depth normal - xmlp.setDepthNormalFeatureEstimationMethod( - m_depthNormalFeatureEstimationMethod); - xmlp.setDepthNormalPclPlaneEstimationMethod( - m_depthNormalPclPlaneEstimationMethod); - xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter( - m_depthNormalPclPlaneEstimationRansacMaxIter); - xmlp.setDepthNormalPclPlaneEstimationRansacThreshold( - m_depthNormalPclPlaneEstimationRansacThreshold); + xmlp.setDepthNormalFeatureEstimationMethod(m_depthNormalFeatureEstimationMethod); + xmlp.setDepthNormalPclPlaneEstimationMethod(m_depthNormalPclPlaneEstimationMethod); + xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter(m_depthNormalPclPlaneEstimationRansacMaxIter); + xmlp.setDepthNormalPclPlaneEstimationRansacThreshold(m_depthNormalPclPlaneEstimationRansacThreshold); xmlp.setDepthNormalSamplingStepX(m_depthNormalSamplingStepX); xmlp.setDepthNormalSamplingStepY(m_depthNormalSamplingStepY); @@ -5222,8 +4654,7 @@ void vpMbGenericTracker::TrackerWrapper::loadConfigFile( std::vector tracker_names; if (m_trackerType & EDGE_TRACKER) tracker_names.push_back("Edge"); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) tracker_names.push_back("Klt"); #endif @@ -5242,8 +4673,7 @@ void vpMbGenericTracker::TrackerWrapper::loadConfigFile( std::cout << "Model-Based Tracker ************ " << std::endl; xmlp.parse(configFile.c_str()); } catch (...) { - throw vpException(vpException::ioError, "Can't open XML file \"%s\"\n ", - configFile.c_str()); + throw vpException(vpException::ioError, "Can't open XML file \"%s\"\n ", configFile.c_str()); } vpCameraParameters camera; @@ -5281,8 +4711,7 @@ void vpMbGenericTracker::TrackerWrapper::loadConfigFile( vpMbEdgeTracker::setMovingEdge(meParser); // KLT -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) tracker.setMaxFeatures((int)xmlp.getKltMaxFeatures()); tracker.setWindowSize((int)xmlp.getKltWindowSize()); tracker.setQuality(xmlp.getKltQuality()); @@ -5297,30 +4726,22 @@ void vpMbGenericTracker::TrackerWrapper::loadConfigFile( #endif // Depth normal - setDepthNormalFeatureEstimationMethod( - xmlp.getDepthNormalFeatureEstimationMethod()); - setDepthNormalPclPlaneEstimationMethod( - xmlp.getDepthNormalPclPlaneEstimationMethod()); - setDepthNormalPclPlaneEstimationRansacMaxIter( - xmlp.getDepthNormalPclPlaneEstimationRansacMaxIter()); - setDepthNormalPclPlaneEstimationRansacThreshold( - xmlp.getDepthNormalPclPlaneEstimationRansacThreshold()); - setDepthNormalSamplingStep(xmlp.getDepthNormalSamplingStepX(), - xmlp.getDepthNormalSamplingStepY()); + setDepthNormalFeatureEstimationMethod(xmlp.getDepthNormalFeatureEstimationMethod()); + setDepthNormalPclPlaneEstimationMethod(xmlp.getDepthNormalPclPlaneEstimationMethod()); + setDepthNormalPclPlaneEstimationRansacMaxIter(xmlp.getDepthNormalPclPlaneEstimationRansacMaxIter()); + setDepthNormalPclPlaneEstimationRansacThreshold(xmlp.getDepthNormalPclPlaneEstimationRansacThreshold()); + setDepthNormalSamplingStep(xmlp.getDepthNormalSamplingStepX(), xmlp.getDepthNormalSamplingStepY()); // Depth dense - setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), - xmlp.getDepthDenseSamplingStepY()); + setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), xmlp.getDepthDenseSamplingStepY()); #else - std::cerr << "You need the libXML2 to read the config file: " << configFile - << std::endl; + std::cerr << "You need the libXML2 to read the config file: " << configFile << std::endl; #endif } #ifdef VISP_HAVE_PCL -void vpMbGenericTracker::TrackerWrapper::postTracking( - const vpImage *const ptr_I, - const pcl::PointCloud::ConstPtr &point_cloud) +void vpMbGenericTracker::TrackerWrapper::postTracking(const vpImage *const ptr_I, + const pcl::PointCloud::ConstPtr &point_cloud) { if (displayFeatures) { if (m_trackerType & EDGE_TRACKER) { @@ -5328,8 +4749,7 @@ void vpMbGenericTracker::TrackerWrapper::postTracking( } } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) // KLT if (m_trackerType & KLT_TRACKER) { if (vpMbKltTracker::postTracking(*ptr_I, m_w_klt)) { @@ -5351,13 +4771,11 @@ void vpMbGenericTracker::TrackerWrapper::postTracking( // Depth normal if (m_trackerType & DEPTH_NORMAL_TRACKER) - vpMbDepthNormalTracker::computeVisibility(point_cloud->width, - point_cloud->height); + vpMbDepthNormalTracker::computeVisibility(point_cloud->width, point_cloud->height); // Depth dense if (m_trackerType & DEPTH_DENSE_TRACKER) - vpMbDepthDenseTracker::computeVisibility(point_cloud->width, - point_cloud->height); + vpMbDepthDenseTracker::computeVisibility(point_cloud->width, point_cloud->height); // Edge if (m_trackerType & EDGE_TRACKER) { @@ -5373,9 +4791,8 @@ void vpMbGenericTracker::TrackerWrapper::postTracking( } } -void vpMbGenericTracker::TrackerWrapper::preTracking( - const vpImage *const ptr_I, - const pcl::PointCloud::ConstPtr &point_cloud) +void vpMbGenericTracker::TrackerWrapper::preTracking(const vpImage *const ptr_I, + const pcl::PointCloud::ConstPtr &point_cloud) { if (m_trackerType & EDGE_TRACKER) { try { @@ -5386,8 +4803,7 @@ void vpMbGenericTracker::TrackerWrapper::preTracking( } } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) { try { vpMbKltTracker::preTracking(*ptr_I); @@ -5418,9 +4834,9 @@ void vpMbGenericTracker::TrackerWrapper::preTracking( } #endif -void vpMbGenericTracker::TrackerWrapper::postTracking( - const vpImage *const ptr_I, - const unsigned int pointcloud_width, const unsigned int pointcloud_height) +void vpMbGenericTracker::TrackerWrapper::postTracking(const vpImage *const ptr_I, + const unsigned int pointcloud_width, + const unsigned int pointcloud_height) { if (displayFeatures) { if (m_trackerType & EDGE_TRACKER) { @@ -5428,8 +4844,7 @@ void vpMbGenericTracker::TrackerWrapper::postTracking( } } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) // KLT if (m_trackerType & KLT_TRACKER) { if (vpMbKltTracker::postTracking(*ptr_I, m_w_klt)) { @@ -5451,13 +4866,11 @@ void vpMbGenericTracker::TrackerWrapper::postTracking( // Depth normal if (m_trackerType & DEPTH_NORMAL_TRACKER) - vpMbDepthNormalTracker::computeVisibility(pointcloud_width, - pointcloud_height); + vpMbDepthNormalTracker::computeVisibility(pointcloud_width, pointcloud_height); // Depth dense if (m_trackerType & DEPTH_DENSE_TRACKER) - vpMbDepthDenseTracker::computeVisibility(pointcloud_width, - pointcloud_height); + vpMbDepthDenseTracker::computeVisibility(pointcloud_width, pointcloud_height); // Edge if (m_trackerType & EDGE_TRACKER) { @@ -5473,10 +4886,10 @@ void vpMbGenericTracker::TrackerWrapper::postTracking( } } -void vpMbGenericTracker::TrackerWrapper::preTracking( - const vpImage *const ptr_I, - const std::vector *const point_cloud, - const unsigned int pointcloud_width, const unsigned int pointcloud_height) +void vpMbGenericTracker::TrackerWrapper::preTracking(const vpImage *const ptr_I, + const std::vector *const point_cloud, + const unsigned int pointcloud_width, + const unsigned int pointcloud_height) { if (m_trackerType & EDGE_TRACKER) { try { @@ -5487,8 +4900,7 @@ void vpMbGenericTracker::TrackerWrapper::preTracking( } } -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) { try { vpMbKltTracker::preTracking(*ptr_I); @@ -5501,8 +4913,7 @@ void vpMbGenericTracker::TrackerWrapper::preTracking( if (m_trackerType & DEPTH_NORMAL_TRACKER) { try { - vpMbDepthNormalTracker::segmentPointCloud( - *point_cloud, pointcloud_width, pointcloud_height); + vpMbDepthNormalTracker::segmentPointCloud(*point_cloud, pointcloud_width, pointcloud_height); } catch (...) { std::cerr << "Error in Depth tracking" << std::endl; throw; @@ -5511,8 +4922,7 @@ void vpMbGenericTracker::TrackerWrapper::preTracking( if (m_trackerType & DEPTH_DENSE_TRACKER) { try { - vpMbDepthDenseTracker::segmentPointCloud(*point_cloud, pointcloud_width, - pointcloud_height); + vpMbDepthDenseTracker::segmentPointCloud(*point_cloud, pointcloud_width, pointcloud_height); } catch (...) { std::cerr << "Error in Depth dense tracking" << std::endl; throw; @@ -5520,9 +4930,8 @@ void vpMbGenericTracker::TrackerWrapper::preTracking( } } -void vpMbGenericTracker::TrackerWrapper::reInitModel( - const vpImage &I, const std::string &cad_name, - const vpHomogeneousMatrix &cMo_, const bool verbose) +void vpMbGenericTracker::TrackerWrapper::reInitModel(const vpImage &I, const std::string &cad_name, + const vpHomogeneousMatrix &cMo_, const bool verbose) { cMo.eye(); @@ -5533,27 +4942,22 @@ void vpMbGenericTracker::TrackerWrapper::reInitModel( for (unsigned int i = 0; i < scales.size(); i++) { if (scales[i]) { - for (std::list::const_iterator it = - lines[i].begin(); - it != lines[i].end(); ++it) { + for (std::list::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) { l = *it; if (l != NULL) delete l; l = NULL; } - for (std::list::const_iterator it = - cylinders[i].begin(); - it != cylinders[i].end(); ++it) { + for (std::list::const_iterator it = cylinders[i].begin(); it != cylinders[i].end(); + ++it) { cy = *it; if (cy != NULL) delete cy; cy = NULL; } - for (std::list::const_iterator it = - circles[i].begin(); - it != circles[i].end(); ++it) { + for (std::list::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) { ci = *it; if (ci != NULL) delete ci; @@ -5572,8 +4976,7 @@ void vpMbGenericTracker::TrackerWrapper::reInitModel( nbvisiblepolygone = 0; // KLT -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) #if (VISP_HAVE_OPENCV_VERSION < 0x020408) if (cur != NULL) { cvReleaseImage(&cur); @@ -5582,9 +4985,7 @@ void vpMbGenericTracker::TrackerWrapper::reInitModel( #endif // delete the Klt Polygon features - for (std::list::const_iterator it = - kltPolygons.begin(); - it != kltPolygons.end(); ++it) { + for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { vpMbtDistanceKltPoints *kltpoly = *it; if (kltpoly != NULL) { delete kltpoly; @@ -5593,9 +4994,8 @@ void vpMbGenericTracker::TrackerWrapper::reInitModel( } kltPolygons.clear(); - for (std::list::const_iterator it = - kltCylinders.begin(); - it != kltCylinders.end(); ++it) { + for (std::list::const_iterator it = kltCylinders.begin(); it != kltCylinders.end(); + ++it) { vpMbtDistanceKltCylinder *kltPolyCylinder = *it; if (kltPolyCylinder != NULL) { delete kltPolyCylinder; @@ -5605,9 +5005,7 @@ void vpMbGenericTracker::TrackerWrapper::reInitModel( kltCylinders.clear(); // delete the structures used to display circles - for (std::list::const_iterator it = - circles_disp.begin(); - it != circles_disp.end(); ++it) { + for (std::list::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) { ci = *it; if (ci != NULL) { delete ci; @@ -5643,42 +5041,33 @@ void vpMbGenericTracker::TrackerWrapper::reInitModel( void vpMbGenericTracker::TrackerWrapper::resetTracker() { vpMbEdgeTracker::resetTracker(); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) vpMbKltTracker::resetTracker(); #endif vpMbDepthNormalTracker::resetTracker(); vpMbDepthDenseTracker::resetTracker(); } -void vpMbGenericTracker::TrackerWrapper::setCameraParameters( - const vpCameraParameters &camera) +void vpMbGenericTracker::TrackerWrapper::setCameraParameters(const vpCameraParameters &camera) { this->cam = camera; vpMbEdgeTracker::setCameraParameters(cam); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) vpMbKltTracker::setCameraParameters(cam); #endif vpMbDepthNormalTracker::setCameraParameters(cam); vpMbDepthDenseTracker::setCameraParameters(cam); } -void vpMbGenericTracker::TrackerWrapper::setClipping( - const unsigned int &flags) -{ - vpMbEdgeTracker::setClipping(flags); -} +void vpMbGenericTracker::TrackerWrapper::setClipping(const unsigned int &flags) { vpMbEdgeTracker::setClipping(flags); } -void vpMbGenericTracker::TrackerWrapper::setFarClippingDistance( - const double &dist) +void vpMbGenericTracker::TrackerWrapper::setFarClippingDistance(const double &dist) { vpMbEdgeTracker::setFarClippingDistance(dist); } -void vpMbGenericTracker::TrackerWrapper::setNearClippingDistance( - const double &dist) +void vpMbGenericTracker::TrackerWrapper::setNearClippingDistance(const double &dist) { vpMbEdgeTracker::setNearClippingDistance(dist); } @@ -5691,13 +5080,11 @@ void vpMbGenericTracker::TrackerWrapper::setOgreVisibilityTest(const bool &v) #endif } -void vpMbGenericTracker::TrackerWrapper::setPose( - const vpImage &I, const vpHomogeneousMatrix &cdMo) +void vpMbGenericTracker::TrackerWrapper::setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo) { bool performKltSetPose = false; -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) if (m_trackerType & KLT_TRACKER) { performKltSetPose = true; @@ -5750,18 +5137,15 @@ void vpMbGenericTracker::TrackerWrapper::setPose( vpMbDepthDenseTracker::computeVisibility(I.getWidth(), I.getHeight()); } -void vpMbGenericTracker::TrackerWrapper::setProjectionErrorComputation( - const bool &flag) +void vpMbGenericTracker::TrackerWrapper::setProjectionErrorComputation(const bool &flag) { vpMbEdgeTracker::setProjectionErrorComputation(flag); } -void vpMbGenericTracker::TrackerWrapper::setScanLineVisibilityTest( - const bool &v) +void vpMbGenericTracker::TrackerWrapper::setScanLineVisibilityTest(const bool &v) { vpMbEdgeTracker::setScanLineVisibilityTest(v); -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) vpMbKltTracker::setScanLineVisibilityTest(v); #endif vpMbDepthNormalTracker::setScanLineVisibilityTest(v); @@ -5771,13 +5155,11 @@ void vpMbGenericTracker::TrackerWrapper::setScanLineVisibilityTest( void vpMbGenericTracker::TrackerWrapper::setTrackerType(const int type) { if ((type & (EDGE_TRACKER | -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) KLT_TRACKER | #endif DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) { - throw vpException(vpTrackingException::badValue, - "bad value for tracker type: !", type); + throw vpException(vpTrackingException::badValue, "bad value for tracker type: !", type); } m_trackerType = type; @@ -5797,8 +5179,7 @@ void vpMbGenericTracker::TrackerWrapper::track(const vpImage & ) { if ((m_trackerType & (EDGE_TRACKER -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) | KLT_TRACKER #endif )) == 0) { @@ -5812,13 +5193,11 @@ void vpMbGenericTracker::TrackerWrapper::track(const vpImage & } #ifdef VISP_HAVE_PCL -void vpMbGenericTracker::TrackerWrapper::track( - const vpImage *const ptr_I, - const pcl::PointCloud::ConstPtr &point_cloud) +void vpMbGenericTracker::TrackerWrapper::track(const vpImage *const ptr_I, + const pcl::PointCloud::ConstPtr &point_cloud) { if ((m_trackerType & (EDGE_TRACKER | -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) KLT_TRACKER | #endif DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) { @@ -5827,8 +5206,7 @@ void vpMbGenericTracker::TrackerWrapper::track( } if (m_trackerType & (EDGE_TRACKER -#if defined(VISP_HAVE_MODULE_KLT) && \ - (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) +#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)) | KLT_TRACKER #endif ) && @@ -5836,10 +5214,8 @@ void vpMbGenericTracker::TrackerWrapper::track( throw vpException(vpException::fatalError, "Image pointer is NULL!"); } - if (m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && - point_cloud == nullptr) { - throw vpException(vpException::fatalError, - "Pointcloud smart pointer is NULL!"); + if (m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && point_cloud == nullptr) { + throw vpException(vpException::fatalError, "Pointcloud smart pointer is NULL!"); } // Back-up cMo in case of exception diff --git a/modules/tracker/mbt/src/vpMbScanLine.cpp b/modules/tracker/mbt/src/vpMbScanLine.cpp index 2775b3f27f..828bf5c362 100644 --- a/modules/tracker/mbt/src/vpMbScanLine.cpp +++ b/modules/tracker/mbt/src/vpMbScanLine.cpp @@ -59,8 +59,7 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS vpMbScanLine::vpMbScanLine() - : w(0), h(0), K(), maskBorder(0), mask(), primitive_ids(), - visibility_samples(), depthTreshold(1e-06) + : w(0), h(0), K(), maskBorder(0), mask(), primitive_ids(), visibility_samples(), depthTreshold(1e-06) #if defined(DEBUG_DISP) , dispMaskDebug(NULL), dispLineDebug(NULL), linedebugImg() @@ -94,9 +93,8 @@ vpMbScanLine::~vpMbScanLine() \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 > &scanlines) +void vpMbScanLine::drawLineY(const vpColVector &a, const vpColVector &b, const vpMbScanLineEdge &edge, const int ID, + std::vector > &scanlines) { double x0 = a[0] / a[2]; double y0 = a[1] / a[2]; @@ -111,12 +109,10 @@ void vpMbScanLine::drawLineY( } // if (y0 >= h - 1 || y1 < 0 || y1 == y0) - if (y0 >= h - 1 || y1 < 0 || - std::fabs(y1 - y0) <= std::numeric_limits::epsilon()) + if (y0 >= h - 1 || y1 < 0 || std::fabs(y1 - y0) <= std::numeric_limits::epsilon()) return; - const unsigned int _y0 = - (std::max)((unsigned int)0, (unsigned int)(std::ceil(y0))); + const unsigned int _y0 = (std::max)((unsigned int)0, (unsigned int)(std::ceil(y0))); const double _y1 = (std::min)((double)h, (double)y1); const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1)); @@ -146,9 +142,8 @@ void vpMbScanLine::drawLineY( \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 > &scanlines) +void vpMbScanLine::drawLineX(const vpColVector &a, const vpColVector &b, const vpMbScanLineEdge &edge, const int ID, + std::vector > &scanlines) { double x0 = a[0] / a[2]; double y0 = a[1] / a[2]; @@ -163,12 +158,10 @@ void vpMbScanLine::drawLineX( } // if (x0 >= w - 1 || x1 < 0 || x1 == x0) - if (x0 >= w - 1 || x1 < 0 || - std::fabs(x1 - x0) <= std::numeric_limits::epsilon()) + if (x0 >= w - 1 || x1 < 0 || std::fabs(x1 - x0) <= std::numeric_limits::epsilon()) return; - const unsigned int _x0 = - (std::max)((unsigned int)0, (unsigned int)(std::ceil(x0))); + const unsigned int _x0 = (std::max)((unsigned int)0, (unsigned int)(std::ceil(x0))); const double _x1 = (std::min)((double)w, (double)x1); const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1)); @@ -195,9 +188,8 @@ void vpMbScanLine::drawLineX( \param ID : ID of the polygon (has to be know when using queries). \param scanlines : Resulting intersections. */ -void vpMbScanLine::drawPolygonY( - const std::vector > &polygon, - const int ID, std::vector > &scanlines) +void vpMbScanLine::drawPolygonY(const std::vector > &polygon, const int ID, + std::vector > &scanlines) { if (polygon.size() < 2) return; @@ -207,9 +199,7 @@ void vpMbScanLine::drawPolygonY( createVectorFromPoint(polygon.front().first, p1, K); createVectorFromPoint(polygon.back().first, p2, K); - drawLineY(p1, p2, - makeMbScanLineEdge(polygon.front().first, polygon.back().first), - ID, scanlines); + drawLineY(p1, p2, makeMbScanLineEdge(polygon.front().first, polygon.back().first), ID, scanlines); return; } @@ -221,10 +211,8 @@ void vpMbScanLine::drawPolygonY( 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); + drawLineY(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID, + local_scanlines); } createScanLinesFromLocals(scanlines, local_scanlines, h); @@ -237,9 +225,8 @@ void vpMbScanLine::drawPolygonY( \param ID : ID of the polygon (has to be know when using queries). \param scanlines : Resulting intersections. */ -void vpMbScanLine::drawPolygonX( - const std::vector > &polygon, - const int ID, std::vector > &scanlines) +void vpMbScanLine::drawPolygonX(const std::vector > &polygon, const int ID, + std::vector > &scanlines) { if (polygon.size() < 2) return; @@ -249,9 +236,7 @@ void vpMbScanLine::drawPolygonX( createVectorFromPoint(polygon.front().first, p1, K); createVectorFromPoint(polygon.back().first, p2, K); - drawLineX(p1, p2, - makeMbScanLineEdge(polygon.front().first, polygon.back().first), - ID, scanlines); + drawLineX(p1, p2, makeMbScanLineEdge(polygon.front().first, polygon.back().first), ID, scanlines); return; } @@ -263,10 +248,8 @@ void vpMbScanLine::drawPolygonX( 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); + drawLineX(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID, + local_scanlines); } createScanLinesFromLocals(scanlines, local_scanlines, w); @@ -281,10 +264,9 @@ void vpMbScanLine::drawPolygonX( \param localScanlines : Local scanline vector (X or Y-axis). \param size : Corresponding size (typically the width or the height). */ -void vpMbScanLine::createScanLinesFromLocals( - std::vector > &scanlines, - std::vector > &localScanlines, - const unsigned int &size) +void vpMbScanLine::createScanLinesFromLocals(std::vector > &scanlines, + std::vector > &localScanlines, + const unsigned int &size) { for (unsigned int j = 0; j < size; ++j) { std::vector &scanline = localScanlines[j]; @@ -322,11 +304,9 @@ void vpMbScanLine::createScanLinesFromLocals( 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 > *> - &polygons, - std::vector listPolyIndices, const vpCameraParameters &cam, - unsigned int width, unsigned int height) +void vpMbScanLine::drawScene(const std::vector > *> &polygons, + std::vector listPolyIndices, const vpCameraParameters &cam, unsigned int width, + unsigned int height) { this->w = width; this->h = height; @@ -381,9 +361,7 @@ void vpMbScanLine::drawScene( 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)); + 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()); @@ -408,8 +386,7 @@ void vpMbScanLine::drawScene( // 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 unsigned int x0 = (std::max)((unsigned int)0, (unsigned int)(std::ceil(last_visible.p))); const double x1 = (std::min)((double)w, (double)s.p); for (unsigned int x = x0 + maskBorder; x < x1 - maskBorder; ++x) { primitive_ids[(unsigned int)y][(unsigned int)x] = last_visible.ID; @@ -459,9 +436,7 @@ void vpMbScanLine::drawScene( 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)); + 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()); @@ -486,8 +461,7 @@ void vpMbScanLine::drawScene( // 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 unsigned int y0 = (std::max)((unsigned int)0, (unsigned int)(std::ceil(last_visible.p))); const double y1 = (std::min)((double)h, (double)s.p); for (unsigned int y = y0 + maskBorder; y < y1 - maskBorder; ++y) { // primitive_ids[(unsigned int)y][(unsigned int)x] = @@ -552,10 +526,8 @@ void vpMbScanLine::drawScene( line. \param displayResults : True if the results have to be displayed. False otherwise. */ -void vpMbScanLine::queryLineVisibility( - const vpPoint &a, const vpPoint &b, - std::vector > &lines, - const bool &displayResults) +void vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b, + std::vector > &lines, const bool &displayResults) { vpColVector _a, _b; createVectorFromPoint(a, _a, K); @@ -593,8 +565,7 @@ void vpMbScanLine::queryLineVisibility( 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 + if (std::fabs(y0 - y1) > std::fabs(x0 - x1)) // Test if the biggest difference is on the Y-axis { v0 = &y0; v1 = &y1; @@ -620,8 +591,7 @@ void vpMbScanLine::queryLineVisibility( } // if (*v0 >= size - 1 || *v1 < 0 || *v1 == *v0) - if (*v0 >= size - 1 || *v1 < 0 || - std::fabs(*v1 - *v0) <= std::numeric_limits::epsilon()) + if (*v0 >= size - 1 || *v1 < 0 || std::fabs(*v1 - *v0) <= std::numeric_limits::epsilon()) return; const int _v0 = (std::max)(0, int(std::ceil(*v0))); @@ -632,11 +602,9 @@ void vpMbScanLine::queryLineVisibility( vpPoint line_start; vpPoint line_end; bool b_line_started = false; - for (std::set::const_iterator it = visible_samples.begin(); - it != visible_samples.end(); ++it) { + for (std::set::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 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) { @@ -672,10 +640,8 @@ void vpMbScanLine::queryLineVisibility( 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); + 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); } @@ -692,8 +658,7 @@ void vpMbScanLine::queryLineVisibility( \return Resulting vpMbScanLineEdge. */ -vpMbScanLine::vpMbScanLineEdge -vpMbScanLine::makeMbScanLineEdge(const vpPoint &a, const vpPoint &b) +vpMbScanLine::vpMbScanLineEdge vpMbScanLine::makeMbScanLineEdge(const vpPoint &a, const vpPoint &b) { vpColVector _a(3); vpColVector _b(3); @@ -727,8 +692,7 @@ vpMbScanLine::makeMbScanLineEdge(const vpPoint &a, const vpPoint &b) \param v : Resulting vector. \param K : Camera parameters. */ -void vpMbScanLine::createVectorFromPoint(const vpPoint &p, vpColVector &v, - const vpCameraParameters &K) +void vpMbScanLine::createVectorFromPoint(const vpPoint &p, vpColVector &v, const vpCameraParameters &K) { v = vpColVector(3); @@ -746,8 +710,7 @@ void vpMbScanLine::createVectorFromPoint(const vpPoint &p, vpColVector &v, \param X1 : Second extremity. \param Z1 : Second extremity. */ -double vpMbScanLine::getAlpha(double x, double X0, double Z0, double X1, - double Z1) +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); @@ -769,10 +732,7 @@ double vpMbScanLine::getAlpha(double x, double X0, double Z0, double X1, \return Interpolated value. */ -double vpMbScanLine::mix(double a, double b, double alpha) -{ - return a + (b - a) * alpha; -} +double vpMbScanLine::mix(double a, double b, double alpha) { return a + (b - a) * alpha; } /*! Interpolate two vpPoints. @@ -803,8 +763,7 @@ vpPoint vpMbScanLine::mix(const vpPoint &a, const vpPoint &b, double alpha) */ 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()) + + 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())); } diff --git a/modules/tracker/mbt/src/vpMbTracker.cpp b/modules/tracker/mbt/src/vpMbTracker.cpp index 42942625c9..f79994b410 100644 --- a/modules/tracker/mbt/src/vpMbTracker.cpp +++ b/modules/tracker/mbt/src/vpMbTracker.cpp @@ -90,8 +90,7 @@ #include #endif -#if defined __SSE2__ || defined _M_X64 || \ - (defined _M_IX86_FP && _M_IX86_FP >= 2) +#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2) #include #define VISP_HAVE_SSE2 1 #endif @@ -104,10 +103,7 @@ namespace Structure to store info about segment in CAO model files. */ struct SegmentInfo { - SegmentInfo() - : extremities(), name(), useLod(false), minLineLengthThresh(0.) - { - } + SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {} std::vector extremities; std::string name; @@ -120,16 +116,12 @@ struct SegmentInfo { 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 &corners) + PolygonFaceInfo(const double dist, const vpPolygon &poly, const std::vector &corners) : distanceToCamera(dist), polygon(poly), faceCorners(corners) { } - bool operator<(const PolygonFaceInfo &pfi) const - { - return distanceToCamera < pfi.distanceToCamera; - } + bool operator<(const PolygonFaceInfo &pfi) const { return distanceToCamera < pfi.distanceToCamera; } double distanceToCamera; vpPolygon polygon; @@ -144,21 +136,14 @@ struct PolygonFaceInfo { */ vpMbTracker::vpMbTracker() - : cam(), cMo(), oJo(6, 6), isoJoIdentity(true), modelFileName(), - modelInitialised(false), poseSavingFilename(), computeCovariance(false), - covarianceMatrix(), computeProjError(false), projectionError(90.0), - displayFeatures(false), - 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(), - m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), - m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01) + : cam(), cMo(), oJo(6, 6), isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(), + computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0), + displayFeatures(false), 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(), + m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01) { oJo.eye(); // Map used to parse additional information in CAO model files, @@ -195,7 +180,8 @@ vpMbTracker::~vpMbTracker() {} \param I : Input image where the user has to click. \param initFile : File containing the coordinates of at least 4 3D points the user has to click in the image. This file should have .init extension - (ie teabox.init). \param displayHelp : Optionnal display of an image that + (ie teabox.init). + \param displayHelp : Optionnal display of an image that should have the same generic name as the init file (ie teabox.ppm). This image may be used to show where to click. This functionality is only available if visp_io module is used. @@ -204,9 +190,7 @@ vpMbTracker::~vpMbTracker() {} exist. */ -void vpMbTracker::initClick(const vpImage &I, - const std::string &initFile, - const bool displayHelp) +void vpMbTracker::initClick(const vpImage &I, const std::string &initFile, const bool displayHelp) { vpHomogeneousMatrix last_cMo; vpPoseVector init_pos; @@ -234,8 +218,7 @@ void vpMbTracker::initClick(const vpImage &I, sprintf(s, "%s", poseSavingFilename.c_str()); } if (finitpos.fail()) { - std::cout << "cannot read " << s << std::endl - << "cMo set to identity" << std::endl; + 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) { @@ -255,10 +238,7 @@ void vpMbTracker::initClick(const vpImage &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::displayText(I, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red); vpDisplay::flush(I); @@ -291,8 +271,7 @@ void vpMbTracker::initClick(const vpImage &I, finit.open(s, std::ios::in); if (finit.fail()) { std::cout << "cannot read " << s << std::endl; - throw vpException(vpException::ioError, - "Cannot open model-based tracker init file %s", s); + throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", s); } #ifdef VISP_HAVE_MODULE_IO @@ -306,8 +285,7 @@ void vpMbTracker::initClick(const vpImage &I, dispF = initFile + ".ppm"; if (vpIoTools::checkFilename(dispF)) { - std::cout << "Load image to help initialization: " << dispF - << std::endl; + std::cout << "Load image to help initialization: " << dispF << std::endl; #if defined VISP_HAVE_X11 d_help = new vpDisplayX; #elif defined VISP_HAVE_GDI @@ -318,11 +296,9 @@ void vpMbTracker::initClick(const vpImage &I, vpImage 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..."); +#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 @@ -351,9 +327,7 @@ void vpMbTracker::initClick(const vpImage &I, finit.ignore(256, '\n'); // skip the rest of the line std::cout << "Number of 3D points " << n3d << std::endl; if (n3d > 100000) { - throw vpException( - vpException::badValue, - "In %s file, the number of 3D points exceed the max allowed", s); + throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed", s); } vpPoint *P = new vpPoint[n3d]; @@ -372,8 +346,7 @@ void vpMbTracker::initClick(const vpImage &I, 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; + std::cout << "Point " << i + 1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl; P[i].setWorldCoordinates(X, Y, Z); // (X,Y,Z) } @@ -422,10 +395,7 @@ void vpMbTracker::initClick(const vpImage &I, 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::displayText(I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red); vpDisplay::flush(I); @@ -472,8 +442,7 @@ void vpMbTracker::initClick(const vpImage &I, image used to display the help. This image may be used to show where to click. This functionality is only available if visp_io module is used. */ -void vpMbTracker::initClick(const vpImage &I, - const std::vector &points3D_list, +void vpMbTracker::initClick(const vpImage &I, const std::vector &points3D_list, const std::string &displayFile) { vpDisplay::display(I); @@ -483,16 +452,14 @@ void vpMbTracker::initClick(const vpImage &I, vpPose pose; std::vector 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())); + 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 Iref; // Display window creation and initialisation if (vpIoTools::checkFilename(displayFile)) { try { - std::cout << "Load image to help initialization: " << displayFile - << std::endl; + std::cout << "Load image to help initialization: " << displayFile << std::endl; #if defined VISP_HAVE_X11 d_help = new vpDisplayX; #elif defined VISP_HAVE_GDI @@ -502,11 +469,9 @@ void vpMbTracker::initClick(const vpImage &I, #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..."); +#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 @@ -537,7 +502,7 @@ void vpMbTracker::initClick(const vpImage &I, 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 + pose.addPoint(P[i]); // and added to the pose computation point list } vpDisplay::flush(I); @@ -555,10 +520,7 @@ void vpMbTracker::initClick(const vpImage &I, 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::displayText(I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red); vpDisplay::flush(I); @@ -592,25 +554,25 @@ void vpMbTracker::initClick(const vpImage &I, are allowed. 3D point coordinates are expressed in meter in the object frame with X, Y and Z values. 2D point coordinates are expressied in pixel coordinates, with first the line and then the column of the pixel in the - image. The structure of this file is the following. \code # 3D point - coordinates 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 # / - # corresponding 2D point coordinates - 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 # / + image. The structure of this file is the following. + \code + # 3D point coordinates + 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 # / + # corresponding 2D point coordinates + 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 &I, - const std::string &initFile) +void vpMbTracker::initFromPoints(const vpImage &I, const std::string &initFile) { char s[FILENAME_MAX]; std::fstream finit; @@ -627,8 +589,7 @@ void vpMbTracker::initFromPoints(const vpImage &I, finit.open(s, std::ios::in); if (finit.fail()) { std::cout << "cannot read " << s << std::endl; - throw vpException(vpException::ioError, - "Cannot open model-based tracker init file %s", s); + throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", s); } //******** @@ -648,9 +609,7 @@ void vpMbTracker::initFromPoints(const vpImage &I, finit.ignore(256, '\n'); // skip the rest of the line std::cout << "Number of 3D points " << n3d << std::endl; if (n3d > 100000) { - throw vpException( - vpException::badValue, - "In %s file, the number of 3D points exceed the max allowed", s); + throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed", s); } vpPoint *P = new vpPoint[n3d]; @@ -668,8 +627,7 @@ void vpMbTracker::initFromPoints(const vpImage &I, 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; + std::cout << "Point " << i + 1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl; P[i].setWorldCoordinates(X, Y, Z); // (X,Y,Z) } @@ -690,9 +648,7 @@ void vpMbTracker::initFromPoints(const vpImage &I, std::cout << "Number of 2D points " << n2d << std::endl; if (n2d > 100000) { delete[] P; - throw vpException( - vpException::badValue, - "In %s file, the number of 2D points exceed the max allowed", s); + throw vpException(vpException::badValue, "In %s file, the number of 2D points exceed the max allowed", s); } if (n3d != n2d) { @@ -718,8 +674,7 @@ void vpMbTracker::initFromPoints(const vpImage &I, finit.ignore(256, '\n'); // skip the rest of the line vpImagePoint ip(v, u); - std::cout << "Point " << i + 1 << " with 2D coordinates: " << ip - << std::endl; + std::cout << "Point " << i + 1 << " with 2D coordinates: " << ip << std::endl; vpPixelMeterConversion::convertPoint(cam, ip, x, y); P[i].set_x(x); P[i].set_y(y); @@ -754,10 +709,8 @@ void vpMbTracker::initFromPoints(const vpImage &I, \param points2D_list : List of image points. \param points3D_list : List of 3D points (object frame). */ -void vpMbTracker::initFromPoints( - const vpImage &I, - const std::vector &points2D_list, - const std::vector &points3D_list) +void vpMbTracker::initFromPoints(const vpImage &I, const std::vector &points2D_list, + const std::vector &points3D_list) { if (points2D_list.size() != points3D_list.size()) vpERROR_TRACE("vpMbTracker::initFromPoints(), Number of 2D points " @@ -768,8 +721,7 @@ void vpMbTracker::initFromPoints( 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())); + 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); @@ -810,8 +762,7 @@ void vpMbTracker::initFromPoints( 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 &I, - const std::string &initFile) +void vpMbTracker::initFromPose(const vpImage &I, const std::string &initFile) { char s[FILENAME_MAX]; std::fstream finit; @@ -846,8 +797,7 @@ void vpMbTracker::initFromPose(const vpImage &I, \param I : Input image \param cMo_ : Pose matrix. */ -void vpMbTracker::initFromPose(const vpImage &I, - const vpHomogeneousMatrix &cMo_) +void vpMbTracker::initFromPose(const vpImage &I, const vpHomogeneousMatrix &cMo_) { this->cMo = cMo_; init(I); @@ -859,8 +809,7 @@ void vpMbTracker::initFromPose(const vpImage &I, \param I : Input image \param cPo : Pose vector. */ -void vpMbTracker::initFromPose(const vpImage &I, - const vpPoseVector &cPo) +void vpMbTracker::initFromPose(const vpImage &I, const vpPoseVector &cPo) { vpHomogeneousMatrix _cMo(cPo); initFromPose(I, _cMo); @@ -885,24 +834,19 @@ void vpMbTracker::savePose(const std::string &filename) const finitpos.close(); } -void vpMbTracker::addPolygon(const std::vector &corners, - const int idFace, const std::string &polygonName, - const bool useLod, - const double minPolygonAreaThreshold, +void vpMbTracker::addPolygon(const std::vector &corners, const int idFace, const std::string &polygonName, + const bool useLod, const double minPolygonAreaThreshold, const double minLineLengthThreshold) { std::vector 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::epsilon() || + std::fabs(corners[i].get_oX()) * std::numeric_limits::epsilon() || std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) > - std::fabs(corners[i].get_oY()) * - std::numeric_limits::epsilon() || + std::fabs(corners[i].get_oY()) * std::numeric_limits::epsilon() || std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) > - std::fabs(corners[i].get_oZ()) * - std::numeric_limits::epsilon()) { + std::fabs(corners[i].get_oZ()) * std::numeric_limits::epsilon()) { corners_without_duplicates.push_back(corners[i + 1]); } } @@ -939,18 +883,15 @@ void vpMbTracker::addPolygon(const std::vector &corners, if (clippingFlag != vpPolygon3D::NO_CLIPPING) faces.getPolygon().back()->setClipping(clippingFlag); - if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == - vpPolygon3D::NEAR_CLIPPING) + 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, +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; @@ -974,12 +915,9 @@ void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, 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()) + + 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())); + 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 @@ -1010,8 +948,7 @@ void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, // 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)); + vpHomogeneousMatrix cMc_90(vpTranslationVector(), vpRotationMatrix(0, vpMath::rad(90 * i), 0)); w_p = wMc * cMc_90 * c_p; vpPoint w_P; @@ -1027,18 +964,15 @@ void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, if (clippingFlag != vpPolygon3D::NO_CLIPPING) faces.getPolygon().back()->setClipping(clippingFlag); - if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == - vpPolygon3D::NEAR_CLIPPING) + 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) +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 @@ -1068,18 +1002,15 @@ void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, if (clippingFlag != vpPolygon3D::NO_CLIPPING) faces.getPolygon().back()->setClipping(clippingFlag); - if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == - vpPolygon3D::NEAR_CLIPPING) + 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 > &listFaces, const int idFace, - const std::string &polygonName, const bool useLod, - const double minLineLengthThreshold) +void vpMbTracker::addPolygon(const std::vector > &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++) { @@ -1100,12 +1031,10 @@ void vpMbTracker::addPolygon( if (clippingFlag != vpPolygon3D::NO_CLIPPING) faces.getPolygon().back()->setClipping(clippingFlag); - if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == - vpPolygon3D::NEAR_CLIPPING) + if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING) faces.getPolygon().back()->setNearClippingDistance(distNearClip); - if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == - vpPolygon3D::FAR_CLIPPING) + if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING) faces.getPolygon().back()->setFarClippingDistance(distFarClip); id++; @@ -1137,10 +1066,7 @@ is not 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); -} +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 @@ -1173,10 +1099,8 @@ void vpMbTracker::loadModel(const std::string &modelFile, const bool verbose) 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) == '.')) { + 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 vectorOfModelFilename; int startIdFace = (int)faces.size(); nbPoints = 0; @@ -1185,21 +1109,15 @@ void vpMbTracker::loadModel(const std::string &modelFile, const bool verbose) 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) == '.')) { + 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()); + 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()); + throw vpException(vpException::ioError, "Error: File %s doesn't exist", modelFile.c_str()); } this->modelInitialised = true; @@ -1247,8 +1165,7 @@ void vpMbTracker::loadVRMLModel(const std::string &modelFile) if (!ok) { vpERROR_TRACE("can't open file to load model"); - throw vpException(vpException::fatalError, - "can't open file to load model"); + throw vpException(vpException::fatalError, "can't open file to load model"); } if (!in.isFileVRML2()) { @@ -1278,10 +1195,8 @@ void vpMbTracker::loadVRMLModel(const std::string &modelFile) 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"); + 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 } @@ -1300,8 +1215,7 @@ void vpMbTracker::removeComment(std::ifstream &fileId) fileId.unget(); } -std::map -vpMbTracker::parseParameters(std::string &endLine) +std::map vpMbTracker::parseParameters(std::string &endLine) { std::map mapOfParams; @@ -1309,8 +1223,7 @@ vpMbTracker::parseParameters(std::string &endLine) while (!endLine.empty() && !exit) { exit = true; - for (std::map::const_iterator it = - mapOfParameterNames.begin(); + for (std::map::const_iterator it = mapOfParameterNames.begin(); it != mapOfParameterNames.end(); ++it) { endLine = trim(endLine); // std::cout << "endLine=" << endLine << std::endl; @@ -1371,41 +1284,39 @@ vpMbTracker::parseParameters(std::string &endLine) # 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. + ... // ... | 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 4 // | Index of the points representing the extremitiesof 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 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 + 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 + 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 + 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 + 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 &vectorOfModelFilename, int &startIdFace, - const bool verbose, const bool parent) +void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector &vectorOfModelFilename, + int &startIdFace, const bool verbose, const bool parent) { std::ifstream fileId; fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit); @@ -1437,15 +1348,14 @@ void vpMbTracker::loadCAOModel( } 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, ..."); + throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> Bad parameter " + "header file : use V0, V1, ..."); } removeComment(fileId); //////////////////////////Read the header part if - ///present////////////////////////// + /// present////////////////////////// std::string line; std::string prefix = "load"; @@ -1466,8 +1376,7 @@ void vpMbTracker::loadCAOModel( std::string headerPath = headerPathRead; if (!vpIoTools::isAbsolutePathname(headerPathRead)) { std::string parentDirectory = vpIoTools::getParent(modelFile); - headerPath = - vpIoTools::createFilePath(parentDirectory, headerPathRead); + headerPath = vpIoTools::createFilePath(parentDirectory, headerPathRead); } // Normalize path @@ -1477,8 +1386,7 @@ void vpMbTracker::loadCAOModel( headerPath = vpIoTools::getAbsolutePathname(headerPath); bool cyclic = false; - for (std::vector::const_iterator it = - vectorOfModelFilename.begin(); + for (std::vector::const_iterator it = vectorOfModelFilename.begin(); it != vectorOfModelFilename.end() && !cyclic; ++it) { if (headerPath == *it) { cyclic = true; @@ -1487,14 +1395,12 @@ void vpMbTracker::loadCAOModel( if (!cyclic) { if (vpIoTools::checkFilename(headerPath)) { - loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, - verbose, false); + 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::cout << "WARNING Cyclic dependency detected with file " << headerPath << " declared in " << modelFile << std::endl; } } else { @@ -1507,7 +1413,7 @@ void vpMbTracker::loadCAOModel( } //////////////////////////Read the point declaration - ///part////////////////////////// + /// part////////////////////////// unsigned int caoNbrPoint; fileId >> caoNbrPoint; fileId.ignore(256, '\n'); // skip the rest of the line @@ -1518,14 +1424,11 @@ void vpMbTracker::loadCAOModel( } if (caoNbrPoint > 100000) { - throw vpException(vpException::badValue, - "Exceed the max number of points in the CAO model."); + 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"); + throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> no points are defined"); } vpPoint *caoPoints = new vpPoint[caoNbrPoint]; @@ -1556,10 +1459,9 @@ void vpMbTracker::loadCAOModel( removeComment(fileId); //////////////////////////Read the segment declaration - ///part////////////////////////// + /// part////////////////////////// // Store in a map the potential segments to add - std::map, SegmentInfo> - segmentTemporaryMap; + std::map, SegmentInfo> segmentTemporaryMap; unsigned int caoNbrLine; fileId >> caoNbrLine; fileId.ignore(256, '\n'); // skip the rest of the line @@ -1572,8 +1474,7 @@ void vpMbTracker::loadCAOModel( if (caoNbrLine > 100000) { delete[] caoPoints; - throw vpException(vpException::badValue, - "Exceed the max number of lines in the CAO model."); + throw vpException(vpException::badValue, "Exceed the max number of lines in the CAO model."); } if (caoNbrLine > 0) @@ -1591,24 +1492,21 @@ void vpMbTracker::loadCAOModel( fileId >> index2; //////////////////////////Read the parameter value if - ///present////////////////////////// + /// present////////////////////////// // Get the end of the line char buffer[256]; fileId.getline(buffer, 256); std::string endLine(buffer); - std::map mapOfParams = - parseParameters(endLine); + std::map mapOfParams = parseParameters(endLine); std::string segmentName = ""; - double minLineLengthThresh = - !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0; + 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()); + minLineLengthThresh = std::atof(mapOfParams["minLineLengthThreshold"].c_str()); } if (mapOfParams.find("useLod") != mapOfParams.end()) { useLod = parseBoolean(mapOfParams["useLod"]); @@ -1639,7 +1537,7 @@ void vpMbTracker::loadCAOModel( removeComment(fileId); //////////////////////////Read the face segment declaration - ///part////////////////////////// + /// 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 @@ -1657,8 +1555,7 @@ void vpMbTracker::loadCAOModel( if (caoNbrPolygonLine > 100000) { delete[] caoPoints; delete[] caoLinePoints; - throw vpException(vpException::badValue, - "Exceed the max number of polygon lines."); + throw vpException(vpException::badValue, "Exceed the max number of polygon lines."); } unsigned int index; @@ -1669,76 +1566,62 @@ void vpMbTracker::loadCAOModel( fileId >> nbLinePol; std::vector corners; if (nbLinePol > 100000) { - throw vpException(vpException::badValue, - "Exceed the max number of lines."); + throw vpException(vpException::badValue, "Exceed the max number of lines."); } for (unsigned int n = 0; n < nbLinePol; n++) { fileId >> index; if (index >= caoNbrLine) { - throw vpException(vpException::badValue, - "Exceed the max number of lines."); + 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 key( - caoLinePoints[2 * index], caoLinePoints[2 * index + 1]); + std::pair key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]); faceSegmentKeyVector.push_back(key); } //////////////////////////Read the parameter value if - ///present////////////////////////// + /// present////////////////////////// // Get the end of the line char buffer[256]; fileId.getline(buffer, 256); std::string endLine(buffer); - std::map mapOfParams = - parseParameters(endLine); + std::map mapOfParams = parseParameters(endLine); std::string polygonName = ""; bool useLod = !applyLodSettingInConfig ? useLodGeneral : false; - double minPolygonAreaThreshold = - !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0; + 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()); + minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str()); } if (mapOfParams.find("useLod") != mapOfParams.end()) { useLod = parseBoolean(mapOfParams["useLod"]); } - addPolygon(corners, idFace++, polygonName, useLod, - minPolygonAreaThreshold, minLineLengthThresholdGeneral); - initFaceFromLines( - *(faces.getPolygon() - .back())); // Init from the last polygon that was added + addPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral); + 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, - SegmentInfo>::const_iterator it = - segmentTemporaryMap.begin(); + for (std::map, 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, + 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 + initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added } } removeComment(fileId); //////////////////////////Read the face point declaration - ///part////////////////////////// + /// part////////////////////////// /* Extract the polygon using the point coordinates (top of the file) */ unsigned int caoNbrPolygonPoint; fileId >> caoNbrPolygonPoint; @@ -1746,13 +1629,11 @@ void vpMbTracker::loadCAOModel( nbPolygonPoints += caoNbrPolygonPoint; if (verbose || vectorOfModelFilename.size() == 1) { - std::cout << "> " << caoNbrPolygonPoint << " polygon points" - << std::endl; + std::cout << "> " << caoNbrPolygonPoint << " polygon points" << std::endl; } if (caoNbrPolygonPoint > 100000) { - throw vpException(vpException::badValue, - "Exceed the max number of polygon point."); + throw vpException(vpException::badValue, "Exceed the max number of polygon point."); } for (unsigned int k = 0; k < caoNbrPolygonPoint; k++) { @@ -1761,52 +1642,44 @@ void vpMbTracker::loadCAOModel( unsigned int nbPointPol; fileId >> nbPointPol; if (nbPointPol > 100000) { - throw vpException(vpException::badValue, - "Exceed the max number of points."); + throw vpException(vpException::badValue, "Exceed the max number of points."); } std::vector 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."); + throw vpException(vpException::badValue, "Exceed the max number of points."); } corners.push_back(caoPoints[index]); } //////////////////////////Read the parameter value if - ///present////////////////////////// + /// present////////////////////////// // Get the end of the line char buffer[256]; fileId.getline(buffer, 256); std::string endLine(buffer); - std::map mapOfParams = - parseParameters(endLine); + std::map mapOfParams = parseParameters(endLine); std::string polygonName = ""; bool useLod = !applyLodSettingInConfig ? useLodGeneral : false; - double minPolygonAreaThreshold = - !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0; + 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()); + 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 + addPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral); + initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added } //////////////////////////Read the cylinder declaration - ///part////////////////////////// + /// part////////////////////////// unsigned int caoNbCylinder; try { removeComment(fileId); @@ -1828,8 +1701,7 @@ void vpMbTracker::loadCAOModel( } if (caoNbCylinder > 100000) { - throw vpException(vpException::badValue, - "Exceed the max number of cylinders."); + throw vpException(vpException::badValue, "Exceed the max number of cylinders."); } for (unsigned int k = 0; k < caoNbCylinder; ++k) { @@ -1842,52 +1714,44 @@ void vpMbTracker::loadCAOModel( fileId >> radius; //////////////////////////Read the parameter value if - ///present////////////////////////// + /// present////////////////////////// // Get the end of the line char buffer[256]; fileId.getline(buffer, 256); std::string endLine(buffer); - std::map mapOfParams = - parseParameters(endLine); + std::map mapOfParams = parseParameters(endLine); std::string polygonName = ""; bool useLod = !applyLodSettingInConfig ? useLodGeneral : false; - double minLineLengthThreshold = - !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0; + 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()); + 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); + addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod, minLineLengthThreshold); std::vector > listFaces; - createCylinderBBox(caoPoints[indexP1], caoPoints[indexP2], radius, - listFaces); - addPolygon(listFaces, idFace, polygonName, useLod, - minLineLengthThreshold); + createCylinderBBox(caoPoints[indexP1], caoPoints[indexP2], radius, listFaces); + addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold); idFace += 4; - initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, - idRevolutionAxis, polygonName); + initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName); } } catch (...) { - std::cerr << "Cannot get the number of cylinders. Defaulting to zero." - << std::endl; + std::cerr << "Cannot get the number of cylinders. Defaulting to zero." << std::endl; caoNbCylinder = 0; } //////////////////////////Read the circle declaration - ///part////////////////////////// + /// part////////////////////////// unsigned int caoNbCircle; try { removeComment(fileId); @@ -1909,8 +1773,7 @@ void vpMbTracker::loadCAOModel( } if (caoNbCircle > 100000) { - throw vpException(vpException::badValue, - "Exceed the max number of cicles."); + throw vpException(vpException::badValue, "Exceed the max number of cicles."); } for (unsigned int k = 0; k < caoNbCircle; ++k) { @@ -1924,42 +1787,34 @@ void vpMbTracker::loadCAOModel( fileId >> indexP3; //////////////////////////Read the parameter value if - ///present////////////////////////// + /// present////////////////////////// // Get the end of the line char buffer[256]; fileId.getline(buffer, 256); std::string endLine(buffer); - std::map mapOfParams = - parseParameters(endLine); + std::map mapOfParams = parseParameters(endLine); std::string polygonName = ""; bool useLod = !applyLodSettingInConfig ? useLodGeneral : false; - double minPolygonAreaThreshold = !applyLodSettingInConfig - ? minPolygonAreaThresholdGeneral - : 2500.0; + 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("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, + addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod, minPolygonAreaThreshold); - initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], - radius, idFace++, polygonName); + initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace++, polygonName); } } catch (...) { - std::cerr << "Cannot get the number of circles. Defaulting to zero." - << std::endl; + std::cerr << "Cannot get the number of circles. Defaulting to zero." << std::endl; caoNbCircle = 0; } @@ -1970,22 +1825,18 @@ void vpMbTracker::loadCAOModel( if (vectorOfModelFilename.size() > 1 && parent) { if (verbose) { - std::cout << "Global information for " - << vpIoTools::getName(modelFile) << " :" << std::endl; + 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 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 << "> " << nbPolygonPoints << " polygon points" << std::endl; std::cout << "> " << nbCylinders << " cylinders" << std::endl; std::cout << "> " << nbCircles << " circles" << std::endl; } @@ -2004,12 +1855,10 @@ void vpMbTracker::loadCAOModel( \param transform : Transformation matrix for this group. \param idFace : Index of the face. */ -void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, - vpHomogeneousMatrix &transform, int &idFace) +void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace) { vpHomogeneousMatrix transformCur; - SoVRMLTransform *sceneGraphVRML2Trasnform = - dynamic_cast(sceneGraphVRML2); + SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast(sceneGraphVRML2); if (sceneGraphVRML2Trasnform) { float rx, ry, rz, rw; sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw); @@ -2067,8 +1916,7 @@ void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, std::string name = child->getName().getString(); for (int j = 0; j < child2list->getLength(); j++) { - if (((SoNode *)child2list->get(j))->getTypeId() == - SoVRMLIndexedFaceSet::getClassTypeId()) { + 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)) { @@ -2077,8 +1925,7 @@ void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, extractFaces(face_set, transform, idFace, name); } } - if (((SoNode *)child2list->get(j))->getTypeId() == - SoVRMLIndexedLineSet::getClassTypeId()) { + if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) { SoVRMLIndexedLineSet *line_set; line_set = (SoVRMLIndexedLineSet *)child2list->get(j); extractLines(line_set, idFace, name); @@ -2097,8 +1944,7 @@ void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, \param idFace : Face id. \param polygonName: Name of the polygon. */ -void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, - vpHomogeneousMatrix &transform, int &idFace, +void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName) { std::vector corners; @@ -2116,9 +1962,7 @@ void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, 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 + initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added corners.resize(0); } } else { @@ -2131,8 +1975,7 @@ void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, pointTransformed = transform * pointTransformed; - pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], - pointTransformed[2]); + pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]); corners.push_back(pt); } } @@ -2152,9 +1995,7 @@ void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, \param idFace : Id of the face. \param polygonName: Name of the polygon. */ -void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, - vpHomogeneousMatrix &transform, - int &idFace, +void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName) { std::vector corners_c1, corners_c2; // points belonging to the @@ -2165,11 +2006,8 @@ void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, 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."); + 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); @@ -2186,8 +2024,7 @@ void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, pointTransformed = transform * pointTransformed; - pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], - pointTransformed[2]); + pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]); if (i < (int)corners_c1.size()) { corners_c1[(unsigned int)i] = pt; @@ -2210,13 +2047,9 @@ void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, double radius_c2 = sqrt(dist.sumSquare()); if (std::fabs(radius_c1 - radius_c2) > - (std::numeric_limits::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."); + (std::numeric_limits::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); @@ -2241,8 +2074,7 @@ void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, \param idFace : Id of the face. \param polygonName: Name of the polygon. */ -void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, - const std::string &polygonName) +void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName) { std::vector corners; corners.resize(0); @@ -2257,9 +2089,7 @@ void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, 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 + initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added corners.resize(0); } } else { @@ -2289,10 +2119,8 @@ void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, vpPoint vpMbTracker::getGravityCenter(const std::vector &pts) const { 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."); + 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; @@ -2322,22 +2150,19 @@ vpPoint vpMbTracker::getGravityCenter(const std::vector &pts) const A pair object containing the list of vpPolygon and the list of face corners. */ std::pair, std::vector > > -vpMbTracker::getPolygonFaces(const bool orderPolygons, - const bool useVisibility, const bool clipPolygon) +vpMbTracker::getPolygonFaces(const bool orderPolygons, const bool useVisibility, const bool clipPolygon) { // Temporary variable to permit to order polygons by distance std::vector polygonsTmp; std::vector > roisPtTmp; // Pair containing the list of vpPolygon and the list of face corners - std::pair, std::vector > > - pairOfPolygonFaces; + std::pair, std::vector > > pairOfPolygonFaces; for (unsigned int i = 0; i < faces.getPolygon().size(); i++) { // A face has at least 3 points if (faces.getPolygon()[i]->nbpt > 2) { - if ((useVisibility && faces.getPolygon()[i]->isvisible) || - !useVisibility) { + if ((useVisibility && faces.getPolygon()[i]->isvisible) || !useVisibility) { std::vector roiPts; if (clipPolygon) { @@ -2380,10 +2205,8 @@ vpMbTracker::getPolygonFaces(const bool orderPolygons, 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])); + 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 @@ -2393,9 +2216,8 @@ vpMbTracker::getPolygonFaces(const bool orderPolygons, roisPtTmp.resize(listOfPolygonFaces.size()); size_t cpt = 0; - for (std::vector::const_iterator - it = listOfPolygonFaces.begin(); - it != listOfPolygonFaces.end(); ++it, cpt++) { + for (std::vector::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end(); + ++it, cpt++) { polygonsTmp[cpt] = it->polygon; roisPtTmp[cpt] = it->faceCorners; } @@ -2438,9 +2260,7 @@ void vpMbTracker::setOgreVisibilityTest(const bool &v) */ void vpMbTracker::setFarClippingDistance(const double &dist) { - if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == - vpPolygon3D::NEAR_CLIPPING && - dist <= distNearClip) + 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) @@ -2486,8 +2306,7 @@ void vpMbTracker::setLod(const bool useLod, const std::string &name) \sa setLod(), setMinPolygonAreaThresh() */ -void vpMbTracker::setMinLineLengthThresh(const double minLineLengthThresh, - const std::string &name) +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) { @@ -2504,8 +2323,7 @@ void vpMbTracker::setMinLineLengthThresh(const double minLineLengthThresh, \sa setLod(), setMinLineLengthThresh() */ -void vpMbTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, - const std::string &name) +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) { @@ -2521,9 +2339,7 @@ void vpMbTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, */ void vpMbTracker::setNearClippingDistance(const double &dist) { - if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == - vpPolygon3D::FAR_CLIPPING && - dist >= distFarClip) + 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) @@ -2555,10 +2371,9 @@ void vpMbTracker::setClipping(const unsigned int &flags) faces[i]->setClipping(clippingFlag); } -void vpMbTracker::computeCovarianceMatrixVVS( - const bool isoJoIdentity_, const vpColVector &w_true, - const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, - const vpMatrix &LVJ_true, const vpColVector &error) +void vpMbTracker::computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, + const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, + const vpMatrix &LVJ_true, const vpColVector &error) { if (computeCovariance) { vpMatrix D; @@ -2567,11 +2382,9 @@ void vpMbTracker::computeCovarianceMatrixVVS( // Note that here the covariance is computed on cMoPrev for time // computation efficiency if (isoJoIdentity_) { - covarianceMatrix = - vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, L_true, D); + covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, L_true, D); } else { - covarianceMatrix = - vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, LVJ_true, D); + covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, LVJ_true, D); } } } @@ -2589,13 +2402,10 @@ void vpMbTracker::computeCovarianceMatrixVVS( \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) const +void vpMbTracker::computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR) const { - if (interaction.getRows() != error.getRows() || - interaction.getCols() != 6) { - throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError, - "Incorrect matrices size in computeJTR."); + if (interaction.getRows() != error.getRows() || interaction.getCols() != 6) { + throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "Incorrect matrices size in computeJTR."); } JTR.resize(6, false); @@ -2641,21 +2451,17 @@ void vpMbTracker::computeJTR(const vpMatrix &interaction, } } -void vpMbTracker::computeVVSCheckLevenbergMarquardt( - const unsigned int iter, vpColVector &error, - const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, - double &mu, bool &reStartFromLastIncrement, vpColVector *const w, - const vpColVector *const m_w_prev) +void vpMbTracker::computeVVSCheckLevenbergMarquardt(const unsigned int iter, vpColVector &error, + const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, + double &mu, bool &reStartFromLastIncrement, vpColVector *const w, + const vpColVector *const m_w_prev) { - if (iter != 0 && - m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT) { - if (error.sumSquare() / (double)error.getRows() > - m_error_prev.sumSquare() / (double)m_error_prev.getRows()) { + if (iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT) { + if (error.sumSquare() / (double)error.getRows() > m_error_prev.sumSquare() / (double)m_error_prev.getRows()) { mu *= 10.0; if (mu > 1.0) - throw vpTrackingException(vpTrackingException::fatalError, - "Optimization diverged"); + throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged"); cMo = cMoPrev; error = m_error_prev; @@ -2667,11 +2473,10 @@ void vpMbTracker::computeVVSCheckLevenbergMarquardt( } } -void vpMbTracker::computeVVSPoseEstimation( - const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L, - vpMatrix <L, vpColVector &R, const vpColVector &error, - vpColVector &error_prev, vpColVector <R, double &mu, vpColVector &v, - const vpColVector *const w, vpColVector *const m_w_prev) +void vpMbTracker::computeVVSPoseEstimation(const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L, + vpMatrix <L, vpColVector &R, const vpColVector &error, + vpColVector &error_prev, vpColVector <R, double &mu, vpColVector &v, + const vpColVector *const w, vpColVector *const m_w_prev) { if (isoJoIdentity_) { LTL = L.AtA(); @@ -2682,10 +2487,7 @@ void vpMbTracker::computeVVSPoseEstimation( vpMatrix LMA(LTL.getRows(), LTL.getCols()); LMA.eye(); vpMatrix LTLmuI = LTL + (LMA * mu); - v = -m_lambda * - LTLmuI.pseudoInverse(LTLmuI.getRows() * - std::numeric_limits::epsilon()) * - LTR; + v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits::epsilon()) * LTR; if (iter != 0) mu /= 10.0; @@ -2698,10 +2500,7 @@ void vpMbTracker::computeVVSPoseEstimation( case vpMbTracker::GAUSS_NEWTON_OPT: default: - v = -m_lambda * - LTL.pseudoInverse(LTL.getRows() * - std::numeric_limits::epsilon()) * - LTR; + v = -m_lambda * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits::epsilon()) * LTR; break; } } else { @@ -2717,10 +2516,7 @@ void vpMbTracker::computeVVSPoseEstimation( vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols()); LMA.eye(); vpMatrix LTLmuI = LVJTLVJ + (LMA * mu); - v = -m_lambda * - LTLmuI.pseudoInverse(LTLmuI.getRows() * - std::numeric_limits::epsilon()) * - LVJTR; + v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits::epsilon()) * LVJTR; v = cVo * v; if (iter != 0) @@ -2733,18 +2529,14 @@ void vpMbTracker::computeVVSPoseEstimation( } case vpMbTracker::GAUSS_NEWTON_OPT: default: - v = -m_lambda * - LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * - std::numeric_limits::epsilon()) * - LVJTR; + v = -m_lambda * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits::epsilon()) * LVJTR; v = cVo * v; break; } } } -void vpMbTracker::computeVVSWeights(vpRobust &robust, - const vpColVector &error, vpColVector &w) +void vpMbTracker::computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w) { if (error.getRows() > 0) robust.MEstimator(vpRobust::TUKEY, error, w); @@ -2800,9 +2592,8 @@ void vpMbTracker::setEstimatedDoF(const vpColVector &v) } } -void vpMbTracker::createCylinderBBox( - const vpPoint &p1, const vpPoint &p2, const double &radius, - std::vector > &listFaces) +void vpMbTracker::createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius, + std::vector > &listFaces) { listFaces.clear(); @@ -2833,8 +2624,7 @@ void vpMbTracker::createCylinderBBox( randomVec[2] = 1.0; axisOrtho = vpColVector::crossProd(axis, randomVec); if (axisOrtho.euclideanNorm() < std::numeric_limits::epsilon()) - throw vpMatrixException(vpMatrixException::badValue, - "Problem in the cylinder definition"); + throw vpMatrixException(vpMatrixException::badValue, "Problem in the cylinder definition"); } } diff --git a/modules/tracker/mbt/src/vpMbXmlParser.cpp b/modules/tracker/mbt/src/vpMbXmlParser.cpp index a8b10736f6..3b3ccf033f 100644 --- a/modules/tracker/mbt/src/vpMbXmlParser.cpp +++ b/modules/tracker/mbt/src/vpMbXmlParser.cpp @@ -51,10 +51,8 @@ */ 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) + : cam(), angleAppear(70), angleDisappear(80), hasNearClipping(false), nearClipping(false), hasFarClipping(false), + farClipping(false), fovClipping(false), useLod(false), minLineLengthThreshold(50.0), minPolygonAreaThreshold(2500.0) { init(); @@ -127,11 +125,9 @@ void vpMbXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) bool face_node = false; bool lod_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case camera: { @@ -156,29 +152,21 @@ void vpMbXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) } 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; + 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; + 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; + std::cout << "lod : min line length threshold : " << minLineLengthThreshold << " (default)" << std::endl; + std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << " (default)" << std::endl; } } @@ -203,11 +191,9 @@ void vpMbXmlParser::read_camera(xmlDocPtr doc, xmlNodePtr node) double d_px = this->cam.get_px(); double d_py = this->cam.get_py(); - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case u0: { @@ -238,26 +224,22 @@ void vpMbXmlParser::read_camera(xmlDocPtr doc, xmlNodePtr node) 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; + 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; + 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; + 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; + std::cout << "camera : py : " << this->cam.get_py() << " (default)" << std::endl; else std::cout << "camera : py : " << this->cam.get_py() << std::endl; } @@ -278,11 +260,9 @@ void vpMbXmlParser::read_face(xmlDocPtr doc, xmlNodePtr node) bool far_clipping_node = false; bool fov_clipping_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case angle_appear: { @@ -320,14 +300,12 @@ void vpMbXmlParser::read_face(xmlDocPtr doc, xmlNodePtr node) } if (!angle_appear_node) - std::cout << "face : Angle Appear : " << angleAppear << " (default)" - << std::endl; + 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; + std::cout << "face : Angle Disappear : " << angleDisappear << " (default)" << std::endl; else std::cout << "face : Angle Disappear : " << angleDisappear << std::endl; @@ -351,11 +329,9 @@ void vpMbXmlParser::read_lod(xmlDocPtr doc, xmlNodePtr node) bool min_line_length_threshold_node = false; bool min_polygon_area_threshold_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case use_lod: @@ -385,18 +361,14 @@ void vpMbXmlParser::read_lod(xmlDocPtr doc, xmlNodePtr node) 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; + std::cout << "lod : min line length threshold : " << minLineLengthThreshold << " (default)" << std::endl; else - std::cout << "lod : min line length threshold : " - << minLineLengthThreshold << std::endl; + 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; + std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << " (default)" << std::endl; else - std::cout << "lod : min polygon area threshold : " - << minPolygonAreaThreshold << std::endl; + std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << std::endl; } #elif !defined(VISP_BUILD_SHARED_LIBS) diff --git a/modules/tracker/mbt/src/vpMbtPolygon.cpp b/modules/tracker/mbt/src/vpMbtPolygon.cpp index 1139f0db30..74aeac5003 100644 --- a/modules/tracker/mbt/src/vpMbtPolygon.cpp +++ b/modules/tracker/mbt/src/vpMbtPolygon.cpp @@ -54,21 +54,18 @@ Basic constructor. */ vpMbtPolygon::vpMbtPolygon() - : index(-1), isvisible(false), isappearing(false), useLod(false), - minLineLengthThresh(50.0), minPolygonAreaThresh(2500.0), name(""), - hasOrientation(true) + : 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), + : 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. + // assignements. } vpMbtPolygon &vpMbtPolygon::operator=(const vpMbtPolygon &mbtp) @@ -106,10 +103,8 @@ vpMbtPolygon::~vpMbtPolygon() {} \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 &I) +bool vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo, + const vpCameraParameters &cam, const vpImage &I) { // std::cout << "Computing angle from MBT Face (cMo, alpha)" << std::endl; @@ -131,8 +126,7 @@ bool vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, 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)); + 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, @@ -169,12 +163,9 @@ bool vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, 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[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(); @@ -258,28 +249,28 @@ set by setMinPolygonAreaThresh(). int main() { -vpImage I; + pImage I; -// Acquire an image -vpImageIo::read(I, "my-image.pgm"); + // Acquire an image + vpImageIo::read(I, "my-image.pgm"); -std::string object = "my-object"; -vpMbEdgeTracker tracker; -tracker.loadConfigFile( object+".xml" ); -tracker.loadModel( object+".cao" ); + 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.setLod(true); + tracker.setMinLineLengthThresh(20.); + tracker.setMinPolygonAreaThresh(20.*20.); -tracker.initClick(I, object+".init" ); + tracker.initClick(I, object+".init" ); -while (true) { - // tracking loop -} -vpXmlParser::cleanup(); + while (true) { + // tracking loop + } + vpXmlParser::cleanup(); -return 0; + return 0; } \endcode diff --git a/modules/tracker/mbt/src/vpMbtXmlGenericParser.cpp b/modules/tracker/mbt/src/vpMbtXmlGenericParser.cpp index d5e735f570..73382ae42e 100644 --- a/modules/tracker/mbt/src/vpMbtXmlGenericParser.cpp +++ b/modules/tracker/mbt/src/vpMbtXmlGenericParser.cpp @@ -48,25 +48,19 @@ vpMbtXmlGenericParser::vpMbtXmlGenericParser(const vpParserType &type) // m_cam(), // - m_angleAppear(70), m_angleDisappear(80), m_hasNearClipping(false), - m_nearClipping(false), m_hasFarClipping(false), m_farClipping(false), - m_fovClipping(false), + m_angleAppear(70), m_angleDisappear(80), m_hasNearClipping(false), m_nearClipping(false), m_hasFarClipping(false), + m_farClipping(false), m_fovClipping(false), // - m_useLod(false), m_minLineLengthThreshold(50.0), - m_minPolygonAreaThreshold(2500.0), + m_useLod(false), m_minLineLengthThreshold(50.0), m_minPolygonAreaThreshold(2500.0), // m_ecm(), // - m_kltMaskBorder(0), m_kltMaxFeatures(0), m_kltWinSize(0), - m_kltQualityValue(0.), m_kltMinDist(0.), m_kltHarrisParam(0.), - m_kltBlockSize(0), m_kltPyramidLevels(0), + m_kltMaskBorder(0), m_kltMaxFeatures(0), m_kltWinSize(0), m_kltQualityValue(0.), m_kltMinDist(0.), + m_kltHarrisParam(0.), m_kltBlockSize(0), m_kltPyramidLevels(0), // - m_depthNormalFeatureEstimationMethod( - vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION), - m_depthNormalPclPlaneEstimationMethod(2), - m_depthNormalPclPlaneEstimationRansacMaxIter(200), - m_depthNormalPclPlaneEstimationRansacThreshold(0.001), - m_depthNormalSamplingStepX(2), m_depthNormalSamplingStepY(2), + m_depthNormalFeatureEstimationMethod(vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION), + m_depthNormalPclPlaneEstimationMethod(2), m_depthNormalPclPlaneEstimationRansacMaxIter(200), + m_depthNormalPclPlaneEstimationRansacThreshold(0.001), m_depthNormalSamplingStepX(2), m_depthNormalSamplingStepY(2), // m_depthDenseSamplingStepX(2), m_depthDenseSamplingStepY(2) { @@ -150,10 +144,7 @@ void vpMbtXmlGenericParser::init() \param filename : File to parse. */ -void vpMbtXmlGenericParser::parse(const std::string &filename) -{ - vpXmlParser::parse(filename); -} +void vpMbtXmlGenericParser::parse(const std::string &filename) { vpXmlParser::parse(filename); } /*! Write info to file. @@ -182,11 +173,9 @@ void vpMbtXmlGenericParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) bool depth_normal_node = false; bool depth_dense_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case camera: @@ -245,91 +234,60 @@ void vpMbtXmlGenericParser::readMainClass(xmlDocPtr doc, xmlNodePtr node) } if (!camera_node) { - std::cout << "camera : u0 : " << m_cam.get_u0() << " (default)" - << std::endl; - std::cout << "camera : v0 : " << m_cam.get_v0() << " (default)" - << std::endl; - std::cout << "camera : px : " << m_cam.get_px() << " (default)" - << std::endl; - std::cout << "camera : py : " << m_cam.get_py() << " (default)" - << std::endl; + std::cout << "camera : u0 : " << m_cam.get_u0() << " (default)" << std::endl; + std::cout << "camera : v0 : " << m_cam.get_v0() << " (default)" << std::endl; + std::cout << "camera : px : " << m_cam.get_px() << " (default)" << std::endl; + std::cout << "camera : py : " << m_cam.get_py() << " (default)" << std::endl; } if (!face_node) { - std::cout << "face : Angle Appear : " << m_angleAppear << " (default)" - << std::endl; - std::cout << "face : Angle Disappear : " << m_angleDisappear - << " (default)" << std::endl; + std::cout << "face : Angle Appear : " << m_angleAppear << " (default)" << std::endl; + std::cout << "face : Angle Disappear : " << m_angleDisappear << " (default)" << std::endl; } if (!lod_node) { std::cout << "lod : use lod : " << m_useLod << " (default)" << std::endl; - std::cout << "lod : min line length threshold : " - << m_minLineLengthThreshold << " (default)" << std::endl; - std::cout << "lod : min polygon area threshold : " - << m_minPolygonAreaThreshold << " (default)" << std::endl; + std::cout << "lod : min line length threshold : " << m_minLineLengthThreshold << " (default)" << std::endl; + std::cout << "lod : min polygon area threshold : " << m_minPolygonAreaThreshold << " (default)" << std::endl; } if (!ecm_node && (m_parserType & EDGE_PARSER)) { - std::cout << "ecm : mask : size : " << m_ecm.getMaskSize() << " (default)" - << std::endl; - std::cout << "ecm : mask : nb_mask : " << m_ecm.getMaskNumber() - << " (default)" << std::endl; - std::cout << "ecm : range : tracking : " << m_ecm.getRange() - << " (default)" << std::endl; - std::cout << "ecm : contrast : threshold : " << m_ecm.getThreshold() - << " (default)" << std::endl; - std::cout << "ecm : contrast : mu1 : " << m_ecm.getMu1() << " (default)" - << std::endl; - std::cout << "ecm : contrast : mu2 : " << m_ecm.getMu2() << " (default)" - << std::endl; - std::cout << "ecm : sample : sample_step : " << m_ecm.getSampleStep() - << " (default)" << std::endl; + std::cout << "ecm : mask : size : " << m_ecm.getMaskSize() << " (default)" << std::endl; + std::cout << "ecm : mask : nb_mask : " << m_ecm.getMaskNumber() << " (default)" << std::endl; + std::cout << "ecm : range : tracking : " << m_ecm.getRange() << " (default)" << std::endl; + std::cout << "ecm : contrast : threshold : " << m_ecm.getThreshold() << " (default)" << std::endl; + std::cout << "ecm : contrast : mu1 : " << m_ecm.getMu1() << " (default)" << std::endl; + std::cout << "ecm : contrast : mu2 : " << m_ecm.getMu2() << " (default)" << std::endl; + std::cout << "ecm : sample : sample_step : " << m_ecm.getSampleStep() << " (default)" << std::endl; } if (!klt_node && (m_parserType & KLT_PARSER)) { - std::cout << "klt : Mask Border : " << m_kltMaskBorder << " (default)" - << std::endl; - std::cout << "klt : Max Features : " << m_kltMaxFeatures << " (default)" - << std::endl; - std::cout << "klt : Windows Size : " << m_kltWinSize << " (default)" - << std::endl; - std::cout << "klt : Quality : " << m_kltQualityValue << " (default)" - << std::endl; - std::cout << "klt : Min Distance : " << m_kltMinDist << " (default)" - << std::endl; - std::cout << "klt : Harris Parameter : " << m_kltHarrisParam - << " (default)" << std::endl; - std::cout << "klt : Block Size : " << m_kltBlockSize << " (default)" - << std::endl; - std::cout << "klt : Pyramid Levels : " << m_kltPyramidLevels - << " (default)" << std::endl; + std::cout << "klt : Mask Border : " << m_kltMaskBorder << " (default)" << std::endl; + std::cout << "klt : Max Features : " << m_kltMaxFeatures << " (default)" << std::endl; + std::cout << "klt : Windows Size : " << m_kltWinSize << " (default)" << std::endl; + std::cout << "klt : Quality : " << m_kltQualityValue << " (default)" << std::endl; + std::cout << "klt : Min Distance : " << m_kltMinDist << " (default)" << std::endl; + std::cout << "klt : Harris Parameter : " << m_kltHarrisParam << " (default)" << std::endl; + std::cout << "klt : Block Size : " << m_kltBlockSize << " (default)" << std::endl; + std::cout << "klt : Pyramid Levels : " << m_kltPyramidLevels << " (default)" << std::endl; } if (!depth_normal_node && (m_parserType & DEPTH_NORMAL_PARSER)) { - std::cout << "depth normal : feature_estimation_method : " - << m_depthNormalFeatureEstimationMethod << " (default)" + std::cout << "depth normal : feature_estimation_method : " << m_depthNormalFeatureEstimationMethod << " (default)" << std::endl; - std::cout << "depth normal : PCL_plane_estimation : method : " - << m_depthNormalPclPlaneEstimationMethod << " (default)" - << std::endl; - std::cout << "depth normal : PCL_plane_estimation : max_iter : " - << m_depthNormalPclPlaneEstimationRansacMaxIter << " (default)" - << std::endl; - std::cout << "depth normal : PCL_plane_estimation : ransac_threshold : " - << m_depthNormalPclPlaneEstimationRansacThreshold + std::cout << "depth normal : PCL_plane_estimation : method : " << m_depthNormalPclPlaneEstimationMethod + << " (default)" << std::endl; + std::cout << "depth normal : PCL_plane_estimation : max_iter : " << m_depthNormalPclPlaneEstimationRansacMaxIter << " (default)" << std::endl; - std::cout << "depth normal : sampling_step : step_X " - << m_depthNormalSamplingStepX << " (default)" << std::endl; - std::cout << "depth normal : sampling_step : step_Y " - << m_depthNormalSamplingStepY << " (default)" << std::endl; + std::cout << "depth normal : PCL_plane_estimation : ransac_threshold : " + << m_depthNormalPclPlaneEstimationRansacThreshold << " (default)" << std::endl; + std::cout << "depth normal : sampling_step : step_X " << m_depthNormalSamplingStepX << " (default)" << std::endl; + std::cout << "depth normal : sampling_step : step_Y " << m_depthNormalSamplingStepY << " (default)" << std::endl; } if (!depth_dense_node && (m_parserType & DEPTH_DENSE_PARSER)) { - std::cout << "depth dense : sampling_step : step_X " - << m_depthDenseSamplingStepX << " (default)" << std::endl; - std::cout << "depth dense : sampling_step : step_Y " - << m_depthDenseSamplingStepY << " (default)" << std::endl; + std::cout << "depth dense : sampling_step : step_X " << m_depthDenseSamplingStepX << " (default)" << std::endl; + std::cout << "depth dense : sampling_step : step_Y " << m_depthDenseSamplingStepY << " (default)" << std::endl; } } @@ -354,11 +312,9 @@ void vpMbtXmlGenericParser::read_camera(xmlDocPtr doc, xmlNodePtr node) double d_px = m_cam.get_px(); double d_py = m_cam.get_py(); - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case u0: @@ -391,26 +347,22 @@ void vpMbtXmlGenericParser::read_camera(xmlDocPtr doc, xmlNodePtr node) m_cam.initPersProjWithoutDistortion(d_px, d_py, d_u0, d_v0); if (!u0_node) - std::cout << "camera : u0 : " << m_cam.get_u0() << " (default)" - << std::endl; + std::cout << "camera : u0 : " << m_cam.get_u0() << " (default)" << std::endl; else std::cout << "camera : u0 : " << m_cam.get_u0() << std::endl; if (!v0_node) - std::cout << "camera : v0 : " << m_cam.get_v0() << " (default)" - << std::endl; + std::cout << "camera : v0 : " << m_cam.get_v0() << " (default)" << std::endl; else std::cout << "camera : v0 : " << m_cam.get_v0() << std::endl; if (!px_node) - std::cout << "camera : px : " << m_cam.get_px() << " (default)" - << std::endl; + std::cout << "camera : px : " << m_cam.get_px() << " (default)" << std::endl; else std::cout << "camera : px : " << m_cam.get_px() << std::endl; if (!py_node) - std::cout << "camera : py : " << m_cam.get_py() << " (default)" - << std::endl; + std::cout << "camera : py : " << m_cam.get_py() << " (default)" << std::endl; else std::cout << "camera : py : " << m_cam.get_py() << std::endl; } @@ -429,17 +381,14 @@ void vpMbtXmlGenericParser::read_depth_normal(xmlDocPtr doc, xmlNodePtr node) bool PCL_plane_estimation_node = false; bool sampling_step_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case feature_estimation_method: m_depthNormalFeatureEstimationMethod = - (vpMbtFaceDepthNormal::vpFeatureEstimationType)xmlReadIntChild( - doc, dataNode); + (vpMbtFaceDepthNormal::vpFeatureEstimationType)xmlReadIntChild(doc, dataNode); feature_estimation_method_node = true; break; @@ -461,30 +410,23 @@ void vpMbtXmlGenericParser::read_depth_normal(xmlDocPtr doc, xmlNodePtr node) } if (!feature_estimation_method_node) - std::cout << "depth normal : feature_estimation_method : " - << m_depthNormalFeatureEstimationMethod << " (default)" + std::cout << "depth normal : feature_estimation_method : " << m_depthNormalFeatureEstimationMethod << " (default)" << std::endl; else - std::cout << "depth normal : feature_estimation_method : " - << m_depthNormalFeatureEstimationMethod << std::endl; + std::cout << "depth normal : feature_estimation_method : " << m_depthNormalFeatureEstimationMethod << std::endl; if (!PCL_plane_estimation_node) { - std::cout << "depth normal : PCL_plane_estimation : method : " - << m_depthNormalPclPlaneEstimationMethod << " (default)" - << std::endl; - std::cout << "depth normal : PCL_plane_estimation : max_iter : " - << m_depthNormalPclPlaneEstimationRansacMaxIter << " (default)" - << std::endl; - std::cout << "depth normal : PCL_plane_estimation : ransac_threshold : " - << m_depthNormalPclPlaneEstimationRansacThreshold + std::cout << "depth normal : PCL_plane_estimation : method : " << m_depthNormalPclPlaneEstimationMethod + << " (default)" << std::endl; + std::cout << "depth normal : PCL_plane_estimation : max_iter : " << m_depthNormalPclPlaneEstimationRansacMaxIter << " (default)" << std::endl; + std::cout << "depth normal : PCL_plane_estimation : ransac_threshold : " + << m_depthNormalPclPlaneEstimationRansacThreshold << " (default)" << std::endl; } if (!sampling_step_node) { - std::cout << "depth normal : sampling_step : step_X " - << m_depthNormalSamplingStepX << " (default)" << std::endl; - std::cout << "depth normal : sampling_step : step_Y " - << m_depthNormalSamplingStepY << " (default)" << std::endl; + std::cout << "depth normal : sampling_step : step_X " << m_depthNormalSamplingStepX << " (default)" << std::endl; + std::cout << "depth normal : sampling_step : step_Y " << m_depthNormalSamplingStepY << " (default)" << std::endl; } } @@ -496,35 +438,29 @@ void vpMbtXmlGenericParser::read_depth_normal(xmlDocPtr doc, xmlNodePtr node) \param doc : Pointer to the document. \param node : Pointer to the node information. */ -void vpMbtXmlGenericParser::read_depth_normal_PCL(xmlDocPtr doc, - xmlNodePtr node) +void vpMbtXmlGenericParser::read_depth_normal_PCL(xmlDocPtr doc, xmlNodePtr node) { bool PCL_plane_estimation_method_node = false; bool PCL_plane_estimation_ransac_max_iter_node = false; bool PCL_plane_estimation_ransac_threshold_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case PCL_plane_estimation_method: - m_depthNormalPclPlaneEstimationMethod = - xmlReadIntChild(doc, dataNode); + m_depthNormalPclPlaneEstimationMethod = xmlReadIntChild(doc, dataNode); PCL_plane_estimation_method_node = true; break; case PCL_plane_estimation_ransac_max_iter: - m_depthNormalPclPlaneEstimationRansacMaxIter = - xmlReadIntChild(doc, dataNode); + m_depthNormalPclPlaneEstimationRansacMaxIter = xmlReadIntChild(doc, dataNode); PCL_plane_estimation_ransac_max_iter_node = true; break; case PCL_plane_estimation_ransac_threshold: - m_depthNormalPclPlaneEstimationRansacThreshold = - xmlReadDoubleChild(doc, dataNode); + m_depthNormalPclPlaneEstimationRansacThreshold = xmlReadDoubleChild(doc, dataNode); PCL_plane_estimation_ransac_threshold_node = true; break; @@ -536,25 +472,22 @@ void vpMbtXmlGenericParser::read_depth_normal_PCL(xmlDocPtr doc, } if (!PCL_plane_estimation_method_node) - std::cout << "depth normal : PCL_plane_estimation : method : " - << m_depthNormalPclPlaneEstimationMethod << " (default)" - << std::endl; + std::cout << "depth normal : PCL_plane_estimation : method : " << m_depthNormalPclPlaneEstimationMethod + << " (default)" << std::endl; else - std::cout << "depth normal : PCL_plane_estimation : method : " - << m_depthNormalPclPlaneEstimationMethod << std::endl; + std::cout << "depth normal : PCL_plane_estimation : method : " << m_depthNormalPclPlaneEstimationMethod + << std::endl; if (!PCL_plane_estimation_ransac_max_iter_node) - std::cout << "depth normal : PCL_plane_estimation : max_iter : " - << m_depthNormalPclPlaneEstimationRansacMaxIter << " (default)" - << std::endl; + std::cout << "depth normal : PCL_plane_estimation : max_iter : " << m_depthNormalPclPlaneEstimationRansacMaxIter + << " (default)" << std::endl; else - std::cout << "depth normal : PCL_plane_estimation : max_iter : " - << m_depthNormalPclPlaneEstimationRansacMaxIter << std::endl; + std::cout << "depth normal : PCL_plane_estimation : max_iter : " << m_depthNormalPclPlaneEstimationRansacMaxIter + << std::endl; if (!PCL_plane_estimation_ransac_threshold_node) std::cout << "depth normal : PCL_plane_estimation : ransac_threshold : " - << m_depthNormalPclPlaneEstimationRansacThreshold - << " (default)" << std::endl; + << m_depthNormalPclPlaneEstimationRansacThreshold << " (default)" << std::endl; else std::cout << "depth normal : PCL_plane_estimation : ransac_threshold : " << m_depthNormalPclPlaneEstimationRansacThreshold << std::endl; @@ -568,17 +501,14 @@ void vpMbtXmlGenericParser::read_depth_normal_PCL(xmlDocPtr doc, \param doc : Pointer to the document. \param node : Pointer to the node information. */ -void vpMbtXmlGenericParser::read_depth_normal_sampling_step(xmlDocPtr doc, - xmlNodePtr node) +void vpMbtXmlGenericParser::read_depth_normal_sampling_step(xmlDocPtr doc, xmlNodePtr node) { bool sampling_step_X_node = false; bool sampling_step_Y_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case depth_sampling_step_X: @@ -599,18 +529,14 @@ void vpMbtXmlGenericParser::read_depth_normal_sampling_step(xmlDocPtr doc, } if (!sampling_step_X_node) - std::cout << "depth normal : sampling_step : step_X : " - << m_depthNormalSamplingStepX << " (default)" << std::endl; + std::cout << "depth normal : sampling_step : step_X : " << m_depthNormalSamplingStepX << " (default)" << std::endl; else - std::cout << "depth normal : sampling_step : step_X : " - << m_depthNormalSamplingStepX << std::endl; + std::cout << "depth normal : sampling_step : step_X : " << m_depthNormalSamplingStepX << std::endl; if (!sampling_step_Y_node) - std::cout << "depth normal : sampling_step : step_Y : " - << m_depthNormalSamplingStepY << " (default)" << std::endl; + std::cout << "depth normal : sampling_step : step_Y : " << m_depthNormalSamplingStepY << " (default)" << std::endl; else - std::cout << "depth normal : sampling_step : step_Y : " - << m_depthNormalSamplingStepY << std::endl; + std::cout << "depth normal : sampling_step : step_Y : " << m_depthNormalSamplingStepY << std::endl; } /*! @@ -625,11 +551,9 @@ void vpMbtXmlGenericParser::read_depth_dense(xmlDocPtr doc, xmlNodePtr node) { bool sampling_step_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case depth_dense_sampling_step: @@ -645,10 +569,8 @@ void vpMbtXmlGenericParser::read_depth_dense(xmlDocPtr doc, xmlNodePtr node) } if (!sampling_step_node) { - std::cout << "depth dense : sampling_step : step_X " - << m_depthDenseSamplingStepX << " (default)" << std::endl; - std::cout << "depth dense : sampling_step : step_Y " - << m_depthDenseSamplingStepY << " (default)" << std::endl; + std::cout << "depth dense : sampling_step : step_X " << m_depthDenseSamplingStepX << " (default)" << std::endl; + std::cout << "depth dense : sampling_step : step_Y " << m_depthDenseSamplingStepY << " (default)" << std::endl; } } @@ -660,17 +582,14 @@ void vpMbtXmlGenericParser::read_depth_dense(xmlDocPtr doc, xmlNodePtr node) \param doc : Pointer to the document. \param node : Pointer to the node of the range information. */ -void vpMbtXmlGenericParser::read_depth_dense_sampling_step(xmlDocPtr doc, - xmlNodePtr node) +void vpMbtXmlGenericParser::read_depth_dense_sampling_step(xmlDocPtr doc, xmlNodePtr node) { bool sampling_step_X_node = false; bool sampling_step_Y_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case depth_dense_sampling_step_X: @@ -691,18 +610,14 @@ void vpMbtXmlGenericParser::read_depth_dense_sampling_step(xmlDocPtr doc, } if (!sampling_step_X_node) - std::cout << "depth dense : sampling_step : step_X : " - << m_depthDenseSamplingStepX << " (default)" << std::endl; + std::cout << "depth dense : sampling_step : step_X : " << m_depthDenseSamplingStepX << " (default)" << std::endl; else - std::cout << "depth dense : sampling_step : step_X : " - << m_depthDenseSamplingStepX << std::endl; + std::cout << "depth dense : sampling_step : step_X : " << m_depthDenseSamplingStepX << std::endl; if (!sampling_step_Y_node) - std::cout << "depth dense : sampling_step : step_Y : " - << m_depthDenseSamplingStepY << " (default)" << std::endl; + std::cout << "depth dense : sampling_step : step_Y : " << m_depthDenseSamplingStepY << " (default)" << std::endl; else - std::cout << "depth dense : sampling_step : step_Y : " - << m_depthDenseSamplingStepY << std::endl; + std::cout << "depth dense : sampling_step : step_Y : " << m_depthDenseSamplingStepY << std::endl; } /*! @@ -720,11 +635,9 @@ void vpMbtXmlGenericParser::read_ecm(xmlDocPtr doc, xmlNodePtr node) bool contrast_node = false; bool sample_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case mask: @@ -755,29 +668,22 @@ void vpMbtXmlGenericParser::read_ecm(xmlDocPtr doc, xmlNodePtr node) } if (!mask_node) { - std::cout << "ecm : mask : size : " << m_ecm.getMaskSize() << " (default)" - << std::endl; - std::cout << "ecm : mask : nb_mask : " << m_ecm.getMaskNumber() - << " (default)" << std::endl; + std::cout << "ecm : mask : size : " << m_ecm.getMaskSize() << " (default)" << std::endl; + std::cout << "ecm : mask : nb_mask : " << m_ecm.getMaskNumber() << " (default)" << std::endl; } if (!range_node) { - std::cout << "ecm : range : tracking : " << m_ecm.getRange() - << " (default)" << std::endl; + std::cout << "ecm : range : tracking : " << m_ecm.getRange() << " (default)" << std::endl; } if (!contrast_node) { - std::cout << "ecm : contrast : threshold " << m_ecm.getThreshold() - << " (default)" << std::endl; - std::cout << "ecm : contrast : mu1 " << m_ecm.getMu1() << " (default)" - << std::endl; - std::cout << "ecm : contrast : mu2 " << m_ecm.getMu2() << " (default)" - << std::endl; + std::cout << "ecm : contrast : threshold " << m_ecm.getThreshold() << " (default)" << std::endl; + std::cout << "ecm : contrast : mu1 " << m_ecm.getMu1() << " (default)" << std::endl; + std::cout << "ecm : contrast : mu2 " << m_ecm.getMu2() << " (default)" << std::endl; } if (!sample_node) { - std::cout << "ecm : sample : sample_step : " << m_ecm.getSampleStep() - << " (default)" << std::endl; + std::cout << "ecm : sample : sample_step : " << m_ecm.getSampleStep() << " (default)" << std::endl; } } @@ -800,11 +706,9 @@ void vpMbtXmlGenericParser::read_ecm_contrast(xmlDocPtr doc, xmlNodePtr node) double d_mu1 = m_ecm.getMu1(); double d_mu2 = m_ecm.getMu2(); - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case edge_threshold: @@ -834,21 +738,17 @@ void vpMbtXmlGenericParser::read_ecm_contrast(xmlDocPtr doc, xmlNodePtr node) m_ecm.setThreshold(d_edge_threshold); if (!edge_threshold_node) - std::cout << "ecm : contrast : threshold " << m_ecm.getThreshold() - << " (default)" << std::endl; + std::cout << "ecm : contrast : threshold " << m_ecm.getThreshold() << " (default)" << std::endl; else - std::cout << "ecm : contrast : threshold " << m_ecm.getThreshold() - << std::endl; + std::cout << "ecm : contrast : threshold " << m_ecm.getThreshold() << std::endl; if (!mu1_node) - std::cout << "ecm : contrast : mu1 " << m_ecm.getMu1() << " (default)" - << std::endl; + std::cout << "ecm : contrast : mu1 " << m_ecm.getMu1() << " (default)" << std::endl; else std::cout << "ecm : contrast : mu1 " << m_ecm.getMu1() << std::endl; if (!mu2_node) - std::cout << "ecm : contrast : mu2 " << m_ecm.getMu2() << " (default)" - << std::endl; + std::cout << "ecm : contrast : mu2 " << m_ecm.getMu2() << " (default)" << std::endl; else std::cout << "ecm : contrast : mu2 " << m_ecm.getMu2() << std::endl; } @@ -870,11 +770,9 @@ void vpMbtXmlGenericParser::read_ecm_mask(xmlDocPtr doc, xmlNodePtr node) unsigned int d_size = m_ecm.getMaskSize(); unsigned int d_nb_mask = m_ecm.getMaskNumber(); - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case size: @@ -904,17 +802,14 @@ void vpMbtXmlGenericParser::read_ecm_mask(xmlDocPtr doc, xmlNodePtr node) m_ecm.setMaskNumber(d_nb_mask); if (!size_node) - std::cout << "ecm : mask : size : " << m_ecm.getMaskSize() << " (default)" - << std::endl; + std::cout << "ecm : mask : size : " << m_ecm.getMaskSize() << " (default)" << std::endl; else std::cout << "ecm : mask : size : " << m_ecm.getMaskSize() << std::endl; if (!nb_mask_node) - std::cout << "ecm : mask : nb_mask : " << m_ecm.getMaskNumber() - << " (default)" << std::endl; + std::cout << "ecm : mask : nb_mask : " << m_ecm.getMaskNumber() << " (default)" << std::endl; else - std::cout << "ecm : mask : nb_mask : " << m_ecm.getMaskNumber() - << std::endl; + std::cout << "ecm : mask : nb_mask : " << m_ecm.getMaskNumber() << std::endl; } /*! @@ -932,11 +827,9 @@ void vpMbtXmlGenericParser::read_ecm_range(xmlDocPtr doc, xmlNodePtr node) // current data values. unsigned int m_range_tracking = m_ecm.getRange(); - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case tracking: @@ -954,8 +847,7 @@ void vpMbtXmlGenericParser::read_ecm_range(xmlDocPtr doc, xmlNodePtr node) m_ecm.setRange(m_range_tracking); if (!tracking_node) - std::cout << "ecm : range : tracking : " << m_ecm.getRange() - << " (default)" << std::endl; + std::cout << "ecm : range : tracking : " << m_ecm.getRange() << " (default)" << std::endl; else std::cout << "ecm : range : tracking : " << m_ecm.getRange() << std::endl; } @@ -975,11 +867,9 @@ void vpMbtXmlGenericParser::read_ecm_sample(xmlDocPtr doc, xmlNodePtr node) // current data values. double d_stp = m_ecm.getSampleStep(); - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case step: @@ -997,11 +887,9 @@ void vpMbtXmlGenericParser::read_ecm_sample(xmlDocPtr doc, xmlNodePtr node) m_ecm.setSampleStep(d_stp); if (!step_node) - std::cout << "ecm : sample : sample_step : " << m_ecm.getSampleStep() - << " (default)" << std::endl; + std::cout << "ecm : sample : sample_step : " << m_ecm.getSampleStep() << " (default)" << std::endl; else - std::cout << "ecm : sample : sample_step : " << m_ecm.getSampleStep() - << std::endl; + std::cout << "ecm : sample : sample_step : " << m_ecm.getSampleStep() << std::endl; } /*! @@ -1022,11 +910,9 @@ void vpMbtXmlGenericParser::read_face(xmlDocPtr doc, xmlNodePtr node) m_hasNearClipping = false; m_hasFarClipping = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case angle_appear: @@ -1067,14 +953,12 @@ void vpMbtXmlGenericParser::read_face(xmlDocPtr doc, xmlNodePtr node) } if (!angle_appear_node) - std::cout << "face : Angle Appear : " << m_angleAppear << " (default)" - << std::endl; + std::cout << "face : Angle Appear : " << m_angleAppear << " (default)" << std::endl; else std::cout << "face : Angle Appear : " << m_angleAppear << std::endl; if (!angle_disappear_node) - std::cout << "face : Angle Disappear : " << m_angleDisappear - << " (default)" << std::endl; + std::cout << "face : Angle Disappear : " << m_angleDisappear << " (default)" << std::endl; else std::cout << "face : Angle Disappear : " << m_angleDisappear << std::endl; @@ -1111,11 +995,9 @@ void vpMbtXmlGenericParser::read_klt(xmlDocPtr doc, xmlNodePtr node) bool size_block_node = false; bool pyramid_lvl_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case mask_border: @@ -1166,50 +1048,42 @@ void vpMbtXmlGenericParser::read_klt(xmlDocPtr doc, xmlNodePtr node) } if (!mask_border_node) - std::cout << "klt : Mask Border : " << m_kltMaskBorder << " (default)" - << std::endl; + std::cout << "klt : Mask Border : " << m_kltMaskBorder << " (default)" << std::endl; else std::cout << "klt : Mask Border : " << m_kltMaskBorder << std::endl; if (!max_features_node) - std::cout << "klt : Max Features : " << m_kltMaxFeatures << " (default)" - << std::endl; + std::cout << "klt : Max Features : " << m_kltMaxFeatures << " (default)" << std::endl; else std::cout << "klt : Max Features : " << m_kltMaxFeatures << std::endl; if (!window_size_node) - std::cout << "klt : Windows Size : " << m_kltWinSize << " (default)" - << std::endl; + std::cout << "klt : Windows Size : " << m_kltWinSize << " (default)" << std::endl; else std::cout << "klt : Windows Size : " << m_kltWinSize << std::endl; if (!quality_node) - std::cout << "klt : Quality : " << m_kltQualityValue << " (default)" - << std::endl; + std::cout << "klt : Quality : " << m_kltQualityValue << " (default)" << std::endl; else std::cout << "klt : Quality : " << m_kltQualityValue << std::endl; if (!min_distance_node) - std::cout << "klt : Min Distance : " << m_kltMinDist << " (default)" - << std::endl; + std::cout << "klt : Min Distance : " << m_kltMinDist << " (default)" << std::endl; else std::cout << "klt : Min Distance : " << m_kltMinDist << std::endl; if (!harris_node) - std::cout << "klt : Harris Parameter : " << m_kltHarrisParam - << " (default)" << std::endl; + std::cout << "klt : Harris Parameter : " << m_kltHarrisParam << " (default)" << std::endl; else std::cout << "klt : Harris Parameter : " << m_kltHarrisParam << std::endl; if (!size_block_node) - std::cout << "klt : Block Size : " << m_kltBlockSize << " (default)" - << std::endl; + std::cout << "klt : Block Size : " << m_kltBlockSize << " (default)" << std::endl; else std::cout << "klt : Block Size : " << m_kltBlockSize << std::endl; if (!pyramid_lvl_node) - std::cout << "klt : Pyramid Levels : " << m_kltPyramidLevels - << " (default)" << std::endl; + std::cout << "klt : Pyramid Levels : " << m_kltPyramidLevels << " (default)" << std::endl; else std::cout << "klt : Pyramid Levels : " << m_kltPyramidLevels << std::endl; } @@ -1220,11 +1094,9 @@ void vpMbtXmlGenericParser::read_lod(xmlDocPtr doc, xmlNodePtr node) bool min_line_length_threshold_node = false; bool min_polygon_area_threshold_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case use_lod: @@ -1255,18 +1127,14 @@ void vpMbtXmlGenericParser::read_lod(xmlDocPtr doc, xmlNodePtr node) std::cout << "lod : use lod : " << m_useLod << std::endl; if (!min_line_length_threshold_node) - std::cout << "lod : min line length threshold : " - << m_minLineLengthThreshold << " (default)" << std::endl; + std::cout << "lod : min line length threshold : " << m_minLineLengthThreshold << " (default)" << std::endl; else - std::cout << "lod : min line length threshold : " - << m_minLineLengthThreshold << std::endl; + std::cout << "lod : min line length threshold : " << m_minLineLengthThreshold << std::endl; if (!min_polygon_area_threshold_node) - std::cout << "lod : min polygon area threshold : " - << m_minPolygonAreaThreshold << " (default)" << std::endl; + std::cout << "lod : min polygon area threshold : " << m_minPolygonAreaThreshold << " (default)" << std::endl; else - std::cout << "lod : min polygon area threshold : " - << m_minPolygonAreaThreshold << std::endl; + std::cout << "lod : min polygon area threshold : " << m_minPolygonAreaThreshold << std::endl; } /*! @@ -1277,8 +1145,7 @@ void vpMbtXmlGenericParser::read_lod(xmlDocPtr doc, xmlNodePtr node) \param doc : Pointer to the document. \param node : Pointer to the node of the sample information. */ -void vpMbtXmlGenericParser::read_sample_deprecated(xmlDocPtr doc, - xmlNodePtr node) +void vpMbtXmlGenericParser::read_sample_deprecated(xmlDocPtr doc, xmlNodePtr node) { bool step_node = false; // bool nb_sample_node = false; @@ -1286,11 +1153,9 @@ void vpMbtXmlGenericParser::read_sample_deprecated(xmlDocPtr doc, // current data values. double d_stp = m_ecm.getSampleStep(); - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::const_iterator iter_data = - nodeMap.find((char *)dataNode->name); + std::map::const_iterator iter_data = nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case step: @@ -1308,15 +1173,12 @@ void vpMbtXmlGenericParser::read_sample_deprecated(xmlDocPtr doc, m_ecm.setSampleStep(d_stp); if (!step_node) - std::cout << "[DEPRECATED] sample : sample_step : " - << m_ecm.getSampleStep() << " (default)" << std::endl; + std::cout << "[DEPRECATED] sample : sample_step : " << m_ecm.getSampleStep() << " (default)" << std::endl; else - std::cout << "[DEPRECATED] sample : sample_step : " - << m_ecm.getSampleStep() << std::endl; + std::cout << "[DEPRECATED] sample : sample_step : " << m_ecm.getSampleStep() << 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; + std::cout << " It should be moved in the ecm node (ecm : sample)." << std::endl; } #elif !defined(VISP_BUILD_SHARED_LIBS) diff --git a/modules/tracker/mbt/test/testTukeyEstimator.cpp b/modules/tracker/mbt/test/testTukeyEstimator.cpp index 3ef7eaf97a..705ba7482e 100644 --- a/modules/tracker/mbt/test/testTukeyEstimator.cpp +++ b/modules/tracker/mbt/test/testTukeyEstimator.cpp @@ -85,17 +85,14 @@ int main(int /*argc*/, const char ** /*argv*/) } t = vpTime::measureTimeMs() - t; - std::cout << "t_robust=" << t_robust << " ms ; t (double)=" << t - << " ; ratio=" << (t_robust / t) << std::endl; + std::cout << "t_robust=" << t_robust << " ms ; t (double)=" << t << " ; ratio=" << (t_robust / t) << std::endl; for (size_t i = 0; i < weights.size(); i++) { - if (!vpMath::equal(weights[i], weights_col[(unsigned int)i], - noise_threshold)) { + if (!vpMath::equal(weights[i], weights_col[(unsigned int)i], noise_threshold)) { std::cerr << "Difference between vpRobust::TUKEY and " "vpMbtTukeyEstimator (double)!" << std::endl; - std::cerr << "weights_col[" << i - << "]=" << weights_col[(unsigned int)i] << std::endl; + std::cerr << "weights_col[" << i << "]=" << weights_col[(unsigned int)i] << std::endl; std::cerr << "weights[" << i << "]=" << weights[i] << std::endl; return EXIT_FAILURE; } @@ -128,17 +125,14 @@ int main(int /*argc*/, const char ** /*argv*/) } t = vpTime::measureTimeMs() - t; - std::cout << "t_robust=" << t_robust << " ms ; t (float)=" << t - << " ; ratio=" << (t_robust / t) << std::endl; + std::cout << "t_robust=" << t_robust << " ms ; t (float)=" << t << " ; ratio=" << (t_robust / t) << std::endl; for (size_t i = 0; i < weights.size(); i++) { - if (!vpMath::equal(weights[i], weights_col[(unsigned int)i], - noise_threshold)) { + if (!vpMath::equal(weights[i], weights_col[(unsigned int)i], noise_threshold)) { std::cerr << "Difference between vpRobust::TUKEY and " "vpMbtTukeyEstimator (float)!" << std::endl; - std::cerr << "weights_col[" << i - << "]=" << weights_col[(unsigned int)i] << std::endl; + std::cerr << "weights_col[" << i << "]=" << weights_col[(unsigned int)i] << std::endl; std::cerr << "weights[" << i << "]=" << weights[i] << std::endl; return EXIT_FAILURE; } @@ -167,26 +161,20 @@ int main(int /*argc*/, const char ** /*argv*/) } t = vpTime::measureTimeMs() - t; - std::cout << "t_robust=" << t_robust << " ms ; t (vpColVector)=" << t - << " ; ratio=" << (t_robust / t) << std::endl; + std::cout << "t_robust=" << t_robust << " ms ; t (vpColVector)=" << t << " ; ratio=" << (t_robust / t) << std::endl; for (size_t i = 0; i < weights.size(); i++) { - if (!vpMath::equal(weights[(unsigned int)i], - weights_col[(unsigned int)i], noise_threshold)) { + if (!vpMath::equal(weights[(unsigned int)i], weights_col[(unsigned int)i], noise_threshold)) { std::cerr << "Difference between vpRobust::TUKEY and " "vpMbtTukeyEstimator (float)!" << std::endl; - std::cerr << "weights_col[" << i - << "]=" << weights_col[(unsigned int)i] << std::endl; - std::cerr << "weights[" << i << "]=" << weights[(unsigned int)i] - << std::endl; + std::cerr << "weights_col[" << i << "]=" << weights_col[(unsigned int)i] << std::endl; + std::cerr << "weights[" << i << "]=" << weights[(unsigned int)i] << std::endl; return EXIT_FAILURE; } } } - std::cout - << "vpMbtTukeyEstimator returns the same values than vpRobust::TUKEY." - << std::endl; + std::cout << "vpMbtTukeyEstimator returns the same values than vpRobust::TUKEY." << std::endl; return EXIT_SUCCESS; } diff --git a/modules/tracker/me/include/visp3/me/vpMeEllipse.h b/modules/tracker/me/include/visp3/me/vpMeEllipse.h index 823c736f1a..901681c6dd 100644 --- a/modules/tracker/me/include/visp3/me/vpMeEllipse.h +++ b/modules/tracker/me/include/visp3/me/vpMeEllipse.h @@ -117,13 +117,10 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker void track(const vpImage &Im); void initTracking(const vpImage &I); - void initTracking(const vpImage &I, const unsigned int n, - vpImagePoint *iP); - void initTracking(const vpImage &I, - const std::vector &iP); - void initTracking(const vpImage &I, const vpImagePoint &ic, - double a_p, double b_p, double e_p, double low_alpha, - double high_alpha); + void initTracking(const vpImage &I, const unsigned int n, vpImagePoint *iP); + void initTracking(const vpImage &I, const std::vector &iP); + void initTracking(const vpImage &I, const vpImagePoint &ic, double a_p, double b_p, double e_p, + double low_alpha, double high_alpha); void display(const vpImage &I, vpColor col); void display(const vpImage &I) { @@ -136,8 +133,7 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker @name Deprecated functions */ //@{ - void initTracking(const vpImage &I, const unsigned int n, - unsigned *i, unsigned *j); + void initTracking(const vpImage &I, const unsigned int n, unsigned *i, unsigned *j); //@} #endif // VISP_BUILD_DEPRECATED_FUNCTIONS @@ -340,24 +336,18 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker */ //@{ void computeAngle(int ip1, int jp1, int ip2, int jp2); - void computeAngle(int ip1, int jp1, double &alpha1, int ip2, int jp2, - double &alpha2); + 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 &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 &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 &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 &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 index 9def314ea0..ad2c9d30ec 100644 --- a/modules/tracker/me/include/visp3/me/vpMeLine.h +++ b/modules/tracker/me/include/visp3/me/vpMeLine.h @@ -94,9 +94,7 @@ 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 @@ -153,8 +151,7 @@ int main() 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); + static void update_indices(double theta, int incr, int i, int j, int &i1, int &i2, int &j1, int &j2); protected: vpMeSite PExt[2]; @@ -196,8 +193,7 @@ class VISP_EXPORT vpMeLine : public vpMeTracker void suppressPoints(); void initTracking(const vpImage &I); - void initTracking(const vpImage &I, const vpImagePoint &ip1, - const vpImagePoint &ip2); + void initTracking(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2); void computeRhoTheta(const vpImage &I); double getRho() const; @@ -229,8 +225,7 @@ class VISP_EXPORT vpMeLine : public vpMeTracker */ inline double getC() const { return c; } - static bool intersection(const vpMeLine &line1, const vpMeLine &line2, - vpImagePoint &ip); + 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 @@ -241,34 +236,23 @@ class VISP_EXPORT vpMeLine : public vpMeTracker \param useIntensityForRho : new value of the flag. */ - inline void computeRhoSignFromIntensity(const bool useIntensityForRho) - { - _useIntensityForRho = useIntensityForRho; - } + inline void computeRhoSignFromIntensity(const bool useIntensityForRho) { _useIntensityForRho = useIntensityForRho; } // Static Functions public: - static void display(const vpImage &I, const vpMeSite &PExt1, - const vpMeSite &PExt2, const double &A, const double &B, - const double &C, const vpColor &color = vpColor::green, + static void display(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A, + const double &B, const double &C, const vpColor &color = vpColor::green, unsigned int thickness = 1); - static void display(const vpImage &I, const vpMeSite &PExt1, - const vpMeSite &PExt2, const double &A, const double &B, - const double &C, const vpColor &color = vpColor::green, + static void display(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A, + const double &B, const double &C, const vpColor &color = vpColor::green, unsigned int thickness = 1); - static void display(const vpImage &I, const vpMeSite &PExt1, - const vpMeSite &PExt2, - const std::list &site_list, const double &A, - const double &B, const double &C, - const vpColor &color = vpColor::green, - unsigned int thickness = 1); - static void display(const vpImage &I, const vpMeSite &PExt1, - const vpMeSite &PExt2, - const std::list &site_list, const double &A, - const double &B, const double &C, - const vpColor &color = vpColor::green, - unsigned int thickness = 1); + static void display(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, + const std::list &site_list, const double &A, const double &B, const double &C, + const vpColor &color = vpColor::green, unsigned int thickness = 1); + static void display(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, + const std::list &site_list, const double &A, const double &B, const double &C, + const vpColor &color = vpColor::green, unsigned int thickness = 1); }; #endif diff --git a/modules/tracker/me/include/visp3/me/vpMeNurbs.h b/modules/tracker/me/include/visp3/me/vpMeNurbs.h index 93dc96db8b..407aa36db7 100644 --- a/modules/tracker/me/include/visp3/me/vpMeNurbs.h +++ b/modules/tracker/me/include/visp3/me/vpMeNurbs.h @@ -166,10 +166,7 @@ class VISP_EXPORT vpMeNurbs : public vpMeTracker \param nb_point : The number of control points used to compute the Nurbs. */ - void setNbControlPoints(const unsigned int nb_point) - { - this->nbControlPoints = nb_point; - } + void setNbControlPoints(const unsigned int nb_point) { this->nbControlPoints = nb_point; } /*! Enables or disables the canny detection used during the extremities @@ -177,10 +174,7 @@ class VISP_EXPORT vpMeNurbs : public vpMeTracker \param enable_canny : if true it enables the canny detection. */ - void setEnableCannyDetection(const bool enable_canny) - { - this->enableCannyDetection = enable_canny; - } + void setEnableCannyDetection(const bool enable_canny) { this->enableCannyDetection = enable_canny; } /*! Enables to set the two thresholds use by the canny detection. @@ -195,8 +189,7 @@ class VISP_EXPORT vpMeNurbs : public vpMeTracker } void initTracking(const vpImage &I); - void initTracking(const vpImage &I, - const std::list &ptList); + void initTracking(const vpImage &I, const std::list &ptList); void track(const vpImage &Im); @@ -219,25 +212,19 @@ class VISP_EXPORT vpMeNurbs : public vpMeTracker void display(const vpImage &I, vpColor col); private: - bool computeFreemanChainElement(const vpImage &I, - vpImagePoint &iP, unsigned int &element); + bool computeFreemanChainElement(const vpImage &I, vpImagePoint &iP, unsigned int &element); - bool hasGoodLevel(const vpImage &I, - const vpImagePoint &iP) const; + bool hasGoodLevel(const vpImage &I, const vpImagePoint &iP) const; - bool isInImage(const vpImage &I, - const vpImagePoint &iP) const; + bool isInImage(const vpImage &I, const vpImagePoint &iP) const; void computeFreemanParameters(unsigned int element, vpImagePoint &diP); - bool farFromImageEdge(const vpImage &I, - const vpImagePoint &iP); + bool farFromImageEdge(const vpImage &I, const vpImagePoint &iP); public: - static void display(const vpImage &I, vpNurbs &n, - vpColor color = vpColor::green); - static void display(const vpImage &I, vpNurbs &n, - vpColor color = vpColor::green); + static void display(const vpImage &I, vpNurbs &n, vpColor color = vpColor::green); + static void display(const vpImage &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 index e97a2de1cc..92c58fbc14 100644 --- a/modules/tracker/me/include/visp3/me/vpMeSite.h +++ b/modules/tracker/me/include/visp3/me/vpMeSite.h @@ -118,8 +118,7 @@ class VISP_EXPORT vpMeSite vpMeSite *getQueryList(const vpImage &I, const int range); - void track(const vpImage &im, const vpMe *me, - const bool test_contraste = true); + void track(const vpImage &im, const vpMe *me, const bool test_contraste = true); /*! Set the angle of tangent at site @@ -206,8 +205,7 @@ class VISP_EXPORT vpMeSite vpMeSite &operator=(const vpMeSite &m); int operator!=(const vpMeSite &m); - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - vpMeSite &vpMeS); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpMeSite &vpMeS); // Static functions /*! @@ -220,8 +218,7 @@ class VISP_EXPORT vpMeSite */ static double distance(const vpMeSite &S1, const vpMeSite &S2) { - return (sqrt(vpMath::sqr(S1.ifloat - S2.ifloat) + - vpMath::sqr(S1.jfloat - S2.jfloat))); + return (sqrt(vpMath::sqr(S1.ifloat - S2.ifloat) + vpMath::sqr(S1.jfloat - S2.jfloat))); } /*! @@ -234,15 +231,12 @@ class VISP_EXPORT vpMeSite */ static double sqrDistance(const vpMeSite &S1, const vpMeSite &S2) { - return (vpMath::sqr(S1.ifloat - S2.ifloat) + - vpMath::sqr(S1.jfloat - S2.jfloat)); + return (vpMath::sqr(S1.ifloat - S2.ifloat) + vpMath::sqr(S1.jfloat - S2.jfloat)); } - static void display(const vpImage &I, const double &i, - const double &j, + static void display(const vpImage &I, const double &i, const double &j, const vpMeSiteState &state = NO_SUPPRESSION); - static void display(const vpImage &I, const double &i, - const double &j, + static void display(const vpImage &I, const double &i, const double &j, const vpMeSiteState &state = NO_SUPPRESSION); // Deprecated @@ -252,8 +246,7 @@ class VISP_EXPORT vpMeSite //! 1 = contrast, 2 = threshold, 3 = M-estimator, 0 = nosupp int suppress; - vp_deprecated void getSign(const vpImage &I, - const int range); + vp_deprecated void getSign(const vpImage &I, const int range); #endif }; diff --git a/modules/tracker/me/include/visp3/me/vpMeTracker.h b/modules/tracker/me/include/visp3/me/vpMeTracker.h index 32818a3d27..2a5865508b 100644 --- a/modules/tracker/me/include/visp3/me/vpMeTracker.h +++ b/modules/tracker/me/include/visp3/me/vpMeTracker.h @@ -97,13 +97,9 @@ class VISP_EXPORT vpMeTracker : public vpTracker virtual void display(const vpImage &I, vpColor col) = 0; virtual void display(const vpImage &I); - void display(const vpImage &I, vpColVector &w, - unsigned int &index_w); + void display(const vpImage &I, vpColVector &w, unsigned int &index_w); - void setDisplay(vpMeSite::vpMeSiteDisplayType select) - { - selectDisplay = select; - } + void setDisplay(vpMeSite::vpMeSiteDisplayType select) { selectDisplay = select; } vpMeTracker &operator=(vpMeTracker &f); diff --git a/modules/tracker/me/include/visp3/me/vpNurbs.h b/modules/tracker/me/include/visp3/me/vpNurbs.h index 16717f40cd..ee44beb81c 100644 --- a/modules/tracker/me/include/visp3/me/vpNurbs.h +++ b/modules/tracker/me/include/visp3/me/vpNurbs.h @@ -101,10 +101,8 @@ class VISP_EXPORT vpNurbs : public vpBSpline // 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 &l_knots, - std::vector &l_controlPoints, + static vpMatrix computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, + std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights); vpMatrix computeCurveDers(double u, unsigned int der); @@ -134,69 +132,48 @@ class VISP_EXPORT vpNurbs : public vpBSpline inline void set_weights(const std::list &list) { weights.clear(); - for (std::list::const_iterator it = list.begin(); - it != list.end(); ++it) { + for (std::list::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 &l_knots, - std::vector &l_controlPoints, - std::vector &l_weights); + static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector &l_knots, + std::vector &l_controlPoints, std::vector &l_weights); vpImagePoint computeCurvePoint(double u); - static vpImagePoint * - computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, - unsigned int l_der, std::vector &l_knots, - std::vector &l_controlPoints, - std::vector &l_weights); + static vpImagePoint *computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, + std::vector &l_knots, std::vector &l_controlPoints, + std::vector &l_weights); 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 &l_knots, - std::vector &l_controlPoints, + static void curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p, + std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights); 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 &l_knots, - std::vector &l_controlPoints, - std::vector &l_weights); + static void refineKnotVectCurve(double *l_x, unsigned int l_r, unsigned int l_p, std::vector &l_knots, + std::vector &l_controlPoints, std::vector &l_weights); 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 &l_knots, - std::vector &l_controlPoints, - std::vector &l_weights); - unsigned int removeCurveKnot(double l_u, unsigned int l_r, - unsigned int l_num, double l_TOL); - - static void globalCurveInterp(std::vector &l_crossingPoints, - unsigned int l_p, - std::vector &l_knots, - std::vector &l_controlPoints, + static unsigned int removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL, unsigned int l_s, + unsigned int l_p, std::vector &l_knots, + std::vector &l_controlPoints, std::vector &l_weights); + unsigned int removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL); + + static void globalCurveInterp(std::vector &l_crossingPoints, unsigned int l_p, + std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights); void globalCurveInterp(vpList &l_crossingPoints); void globalCurveInterp(const std::list &l_crossingPoints); void globalCurveInterp(const std::list &l_crossingPoints); void globalCurveInterp(); - static void globalCurveApprox(std::vector &l_crossingPoints, - unsigned int l_p, unsigned int l_n, - std::vector &l_knots, - std::vector &l_controlPoints, + static void globalCurveApprox(std::vector &l_crossingPoints, unsigned int l_p, unsigned int l_n, + std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights); void globalCurveApprox(vpList &l_crossingPoints, unsigned int n); - void globalCurveApprox(const std::list &l_crossingPoints, - unsigned int n); - void globalCurveApprox(const std::list &l_crossingPoints, - unsigned int n); + void globalCurveApprox(const std::list &l_crossingPoints, unsigned int n); + void globalCurveApprox(const std::list &l_crossingPoints, unsigned int n); void globalCurveApprox(unsigned int n); }; diff --git a/modules/tracker/me/src/moving-edges/vpMe.cpp b/modules/tracker/me/src/moving-edges/vpMe.cpp index 3d489e954d..0c2ed74619 100644 --- a/modules/tracker/me/src/moving-edges/vpMe.cpp +++ b/modules/tracker/me/src/moving-edges/vpMe.cpp @@ -89,8 +89,7 @@ static point point_intersection(droite D1, droite D2) return (I); } -static void recale(point &P, double Xmin, double Ymin, double Xmax, - double Ymax) +static void recale(point &P, double Xmin, double Ymin, double Xmax, double Ymax) { if (vpMath::equal(P.x, Xmin)) P.x = Xmin; // a peu pres => exactement ! @@ -116,8 +115,7 @@ static void permute(point &A, point &B) } // vrai si partie visible -static bool clipping(point A, point B, double Xmin, double Ymin, double Xmax, - double Ymax, point &Ac, +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]; @@ -210,8 +208,7 @@ else { Ac=P[1]; Bc=P[0]; } // leur abscisse x) // calcule la surface relative des 2 portions definies // par le segment PQ sur le carre Xmin,Ymin,Xmax,Ymax // Rem : P,Q tries sur x, et donc seulement 6 cas -static double S_relative(point P, point Q, double Xmin, double Ymin, - double Xmax, double Ymax) +static double S_relative(point P, point Q, double Xmin, double Ymin, double Xmax, double Ymax) { if (Q.x < P.x) // tri le couple de points @@ -222,71 +219,55 @@ static double S_relative(point P, point Q, double Xmin, double Ymin, // 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::epsilon()) && + vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits::epsilon()) && (std::fabs(Q.x - Xmax) <= - vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * - std::numeric_limits::epsilon())) + vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits::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::epsilon()) && + vpMath::maximum(std::fabs(P.y), std::fabs(Ymin)) * std::numeric_limits::epsilon()) && (std::fabs(Q.y - Ymax) <= - vpMath::maximum(std::fabs(Q.y), std::fabs(Ymax)) * - std::numeric_limits::epsilon())) || + vpMath::maximum(std::fabs(Q.y), std::fabs(Ymax)) * std::numeric_limits::epsilon())) || ((std::fabs(Q.y - Ymin) <= - vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * - std::numeric_limits::epsilon()) && + vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * std::numeric_limits::epsilon()) && (std::fabs(P.y - Ymax) <= - vpMath::maximum(std::fabs(P.y), std::fabs(Ymax)) * - std::numeric_limits::epsilon()))) + vpMath::maximum(std::fabs(P.y), std::fabs(Ymax)) * std::numeric_limits::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::epsilon() && + vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits::epsilon() && std::fabs(Q.y - Ymax) <= - vpMath::maximum(std::fabs(Q.y), std::fabs(Ymax)) * - std::numeric_limits::epsilon()) + vpMath::maximum(std::fabs(Q.y), std::fabs(Ymax)) * std::numeric_limits::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::epsilon() && + vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits::epsilon() && std::fabs(Q.y - Ymin) <= - vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * - std::numeric_limits::epsilon()) + vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * std::numeric_limits::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::epsilon() && + vpMath::maximum(std::fabs(P.y), std::fabs(Ymin)) * std::numeric_limits::epsilon() && std::fabs(Q.x - Xmax) <= - vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * - std::numeric_limits::epsilon()) + vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits::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::epsilon() && + vpMath::maximum(std::fabs(P.y), std::fabs(Ymax)) * std::numeric_limits::epsilon() && std::fabs(Q.x - Xmax) <= - vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * - std::numeric_limits::epsilon()) + vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits::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); + 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) +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) @@ -301,18 +282,16 @@ calcul_masques(vpColVector &angle, // definitions des angles theta unsigned int nb_theta = angle.getRows(); for (i_theta = 0; i_theta < nb_theta; i_theta++) { - double theta = - M_PI / 180 * angle[i_theta]; // indice i -> theta(i) en radians - // angle[] dans [0,180[ - double cos_theta = cos(theta); // vecteur directeur de l'ECM - double sin_theta = sin(theta); // associe au masque + double theta = M_PI / 180 * angle[i_theta]; // indice i -> theta(i) en radians + // angle[] dans [0,180[ + double cos_theta = cos(theta); // vecteur directeur de l'ECM + double 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::epsilon()) // => tan(theta) infinie ! + if (std::fabs(angle[i_theta] - 90) <= vpMath::maximum(std::fabs(angle[i_theta]), 90.) * + std::numeric_limits::epsilon()) // => tan(theta) infinie ! { P1.x = 0; P1.y = -(int)n; @@ -384,27 +363,19 @@ 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 << " 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; + 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) + : 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 @@ -414,9 +385,8 @@ vpMe::vpMe() } 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) + : 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; } diff --git a/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp b/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp index 367d5b0a89..9c6571af35 100644 --- a/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp @@ -80,10 +80,8 @@ void computeTheta(double &theta, vpColVector &K, const vpImagePoint &iP) 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.) + : 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 @@ -101,11 +99,9 @@ vpMeEllipse::vpMeEllipse() Copy constructor. */ vpMeEllipse::vpMeEllipse(const vpMeEllipse &meellipse) - : vpMeTracker(meellipse), K(meellipse.K), iPc(meellipse.iPc), a(0.), b(0.), - e(0.), iP1(meellipse.iP1), iP2(meellipse.iP2), alpha1(0), - alpha2(2 * M_PI), ce(0.), se(0.), angle(meellipse.angle), m00(0.), - mu11(0.), mu20(0.), mu02(0.), m10(0.), m01(0.), m11(0.), m02(0.), m20(0.), - thresholdWeight(0.2), expecteddensity(0.) + : vpMeTracker(meellipse), K(meellipse.K), iPc(meellipse.iPc), a(0.), b(0.), e(0.), iP1(meellipse.iP1), + iP2(meellipse.iP2), alpha1(0), alpha2(2 * M_PI), ce(0.), se(0.), angle(meellipse.angle), m00(0.), mu11(0.), + mu20(0.), mu02(0.), m10(0.), m01(0.), m11(0.), m02(0.), m20(0.), thresholdWeight(0.2), expecteddensity(0.) { a = meellipse.a; b = meellipse.b; @@ -153,16 +149,14 @@ vpMeEllipse::~vpMeEllipse() void vpMeEllipse::sample(const vpImage &I) { if (!me) { - throw(vpException(vpException::fatalError, - "Moving edges on ellipse tracking not initialized")); + 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::epsilon()) { + if (std::fabs(me->getSampleStep()) <= std::numeric_limits::epsilon()) { std::cout << "In vpMeEllipse::sample: "; std::cout << "function called with sample step = 0"; // return fatalError; @@ -197,8 +191,7 @@ void vpMeEllipse::sample(const vpImage &I) 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)) { + 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); @@ -233,13 +226,11 @@ void vpMeEllipse::sample(const vpImage &I) void vpMeEllipse::reSample(const vpImage &I) { if (!me) { - throw(vpException(vpException::fatalError, - "Moving edges on ellipse tracking not initialized")); + throw(vpException(vpException::fatalError, "Moving edges on ellipse tracking not initialized")); } unsigned int n = numberOfSignal(); - expecteddensity = - (alpha2 - alpha1) / vpMath::rad((double)me->getSampleStep()); + expecteddensity = (alpha2 - alpha1) / vpMath::rad((double)me->getSampleStep()); if ((double)n < 0.5 * expecteddensity) { sample(I); vpMeTracker::track(I); @@ -284,8 +275,7 @@ void vpMeEllipse::getParameters() 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() + + 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); @@ -314,8 +304,7 @@ void vpMeEllipse::printParameters() \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(const vpImagePoint &pt1, - const vpImagePoint &pt2) +void vpMeEllipse::computeAngle(const vpImagePoint &pt1, const vpImagePoint &pt2) { getParameters(); @@ -338,8 +327,7 @@ void vpMeEllipse::computeAngle(const vpImagePoint &pt1, double j11 = iPc.get_j() + ce * j1 + se * i1; double i11 = iPc.get_i() - se * j1 + ce * i1; - double d = - vpMath::sqr(pt1.get_i() - i11) + vpMath::sqr(pt1.get_j() - j11); + double d = vpMath::sqr(pt1.get_i() - i11) + vpMath::sqr(pt1.get_j() - j11); if (d < dmin1) { dmin1 = d; alpha1 = k; @@ -355,8 +343,7 @@ void vpMeEllipse::computeAngle(const vpImagePoint &pt1, if (alpha2 < alpha1) alpha2 += 2 * M_PI; // else if (alpha2 == alpha1) - else if (std::fabs(alpha2 - alpha1) < - std::fabs(alpha1) * std::numeric_limits::epsilon()) + else if (std::fabs(alpha2 - alpha1) < std::fabs(alpha1) * std::numeric_limits::epsilon()) alpha2 += 2 * M_PI; } @@ -369,8 +356,7 @@ void vpMeEllipse::updateTheta() { vpMeSite p_me; double theta; - for (std::list::iterator it = list.begin(); it != list.end(); - ++it) { + for (std::list::iterator it = list.begin(); it != list.end(); ++it) { p_me = *it; vpImagePoint iP; iP.set_i(p_me.ifloat); @@ -415,8 +401,7 @@ void vpMeEllipse::suppressPoints() void vpMeEllipse::seekExtremities(const vpImage &I) { if (!me) { - throw(vpException(vpException::fatalError, - "Moving edges on ellipse tracking not initialized")); + throw(vpException(vpException::fatalError, "Moving edges on ellipse tracking not initialized")); } int rows = (int)I.getHeight(); @@ -532,8 +517,7 @@ void vpMeEllipse::setExtremities() // Loop through list of sites to track std::list::const_iterator itAngle = angle.begin(); - for (std::list::const_iterator itList = list.begin(); - itList != list.end(); ++itList) { + for (std::list::const_iterator itList = list.begin(); itList != list.end(); ++itList) { vpMeSite s = *itList; // current reference pixel double alpha = *itAngle; if (alpha < alphamin) { @@ -584,16 +568,14 @@ void vpMeEllipse::leastSquare() unsigned int nos_1 = numberOfSignal(); if (list.size() < 3) { - throw(vpException(vpException::dimensionError, - "Not enought moving edges to track the ellipse")); + 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::const_iterator it = list.begin(); - it != list.end(); ++it) { + for (std::list::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); @@ -627,8 +609,7 @@ void vpMeEllipse::leastSquare() } k = 0; - for (std::list::iterator it = list.begin(); it != list.end(); - ++it) { + for (std::list::iterator it = list.begin(); it != list.end(); ++it) { p_me = *it; if (p_me.getState() == vpMeSite::NO_SUPPRESSION) { if (w[k] < thresholdWeight) { @@ -699,8 +680,7 @@ void vpMeEllipse::initTracking(const vpImage &I) \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 &I, - const unsigned int n, vpImagePoint *iP) +void vpMeEllipse::initTracking(const vpImage &I, const unsigned int n, vpImagePoint *iP) { vpMatrix A(n, 5); vpColVector b_(n); @@ -729,8 +709,7 @@ void vpMeEllipse::initTracking(const vpImage &I, computeAngle(iP1, iP2); - expecteddensity = - (alpha2 - alpha1) / vpMath::rad((double)me->getSampleStep()); + expecteddensity = (alpha2 - alpha1) / vpMath::rad((double)me->getSampleStep()); display(I, vpColor::green); sample(I); @@ -755,8 +734,7 @@ void vpMeEllipse::initTracking(const vpImage &I, \param iP : A vector of points belonging to the ellipse edge used to initialize the tracking. */ -void vpMeEllipse::initTracking(const vpImage &I, - const std::vector &iP) +void vpMeEllipse::initTracking(const vpImage &I, const std::vector &iP) { unsigned int n = (unsigned int)(iP.size()); vpMatrix A(n, 5); @@ -786,8 +764,7 @@ void vpMeEllipse::initTracking(const vpImage &I, computeAngle(iP1, iP2); - expecteddensity = - (alpha2 - alpha1) / vpMath::rad((double)me->getSampleStep()); + expecteddensity = (alpha2 - alpha1) / vpMath::rad((double)me->getSampleStep()); display(I, vpColor::green); sample(I); @@ -800,10 +777,8 @@ void vpMeEllipse::initTracking(const vpImage &I, vpDisplay::flush(I); } -void vpMeEllipse::initTracking(const vpImage &I, - const vpImagePoint &ic, double a_p, double b_p, - double e_p, double low_alpha, - double high_alpha) +void vpMeEllipse::initTracking(const vpImage &I, const vpImagePoint &ic, double a_p, double b_p, + double e_p, double low_alpha, double high_alpha) { iPc = ic; a = a_p; @@ -881,12 +856,9 @@ void vpMeEllipse::computeMoments() 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(); + 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; @@ -897,8 +869,7 @@ void vpMeEllipse::computeMoments() /*! * \brief computeAngle */ -void vpMeEllipse::computeAngle(int ip1, int jp1, double &_alpha1, int ip2, - int jp2, double &_alpha2) +void vpMeEllipse::computeAngle(int ip1, int jp1, double &_alpha1, int ip2, int jp2, double &_alpha2) { getParameters(); @@ -938,8 +909,7 @@ void vpMeEllipse::computeAngle(int ip1, int jp1, double &_alpha1, int ip2, if (alpha2 < alpha1) alpha2 += 2 * M_PI; - vpCDEBUG(1) << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " - << alpha2 << std::endl; + vpCDEBUG(1) << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl; } /*! @@ -952,8 +922,7 @@ void vpMeEllipse::computeAngle(int ip1, int jp1, int ip2, int jp2) computeAngle(ip1, jp1, a1, ip2, jp2, a2); } -void vpMeEllipse::initTracking(const vpImage &I, - const unsigned int n, unsigned *i, unsigned *j) +void vpMeEllipse::initTracking(const vpImage &I, const unsigned int n, unsigned *i, unsigned *j) { vpMatrix A(n, 5); vpColVector b_(n); @@ -1022,11 +991,9 @@ void vpMeEllipse::initTracking(const vpImage &I, \param thickness : Thickness of the drawings. */ -void vpMeEllipse::display(const vpImage &I, - const vpImagePoint ¢er, const double &A, - const double &B, const double &E, - const double &smallalpha, const double &highalpha, - const vpColor &color, unsigned int thickness) +void vpMeEllipse::display(const vpImage &I, const vpImagePoint ¢er, const double &A, const double &B, + const double &E, const double &smallalpha, const double &highalpha, const vpColor &color, + unsigned int thickness) { double j1, i1; vpImagePoint iP11; @@ -1102,11 +1069,9 @@ void vpMeEllipse::display(const vpImage &I, \param thickness : Thickness of the drawings. */ -void vpMeEllipse::display(const vpImage &I, - const vpImagePoint ¢er, const double &A, - const double &B, const double &E, - const double &smallalpha, const double &highalpha, - const vpColor &color, unsigned int thickness) +void vpMeEllipse::display(const vpImage &I, const vpImagePoint ¢er, const double &A, const double &B, + const double &E, const double &smallalpha, const double &highalpha, const vpColor &color, + unsigned int thickness) { double j1, i1; vpImagePoint iP11; diff --git a/modules/tracker/me/src/moving-edges/vpMeLine.cpp b/modules/tracker/me/src/moving-edges/vpMeLine.cpp index 824d9a000d..9f46c9353a 100644 --- a/modules/tracker/me/src/moving-edges/vpMeLine.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeLine.cpp @@ -79,16 +79,13 @@ void computeDelta(double &delta, int i1, int j1, int i2, int j2) normalizeAngle(delta); } -static void project(double a, double b, double c, double i, double j, - double &ip, double &jp) +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)); + 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)); + ip = (vpMath::sqr(b) * i - a * b * j - c * a) / (vpMath::sqr(a) + vpMath::sqr(b)); jp = (-c - a * ip) / b; } } @@ -99,8 +96,8 @@ static void project(double a, double b, double c, double i, double j, */ 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.) + : rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1), _useIntensityForRho(true), a(0.), + b(0.), c(0.) { } /*! @@ -109,9 +106,8 @@ vpMeLine::vpMeLine() */ 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.) + : 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; @@ -152,19 +148,16 @@ void vpMeLine::sample(const vpImage &I) { if (!me) { vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized"); - throw(vpTrackingException(vpTrackingException::initializationError, - "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::epsilon()) { + if (std::fabs(me->getSampleStep()) <= std::numeric_limits::epsilon()) { vpERROR_TRACE("function called with sample step = 0"); - throw(vpTrackingException(vpTrackingException::fatalError, - "sample step = 0")); + throw(vpTrackingException(vpTrackingException::fatalError, "sample step = 0")); } // i, j portions of the line_p @@ -173,9 +166,7 @@ void vpMeLine::sample(const vpImage &I) double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj))); if (std::fabs(length_p) <= std::numeric_limits::epsilon()) - throw(vpTrackingException( - vpTrackingException::fatalError, - "points too close of each other to define a line")); + 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(); @@ -295,8 +286,7 @@ void vpMeLine::leastSquare() if (list.size() <= 2 || numberOfSignal() <= 2) { // vpERROR_TRACE("Not enough point") ; vpCDEBUG(1) << "Not enough point"; - throw(vpTrackingException(vpTrackingException::notEnoughPointError, - "not enough point")); + throw(vpTrackingException(vpTrackingException::notEnoughPointError, "not enough point")); } if ((fabs(b) >= 0.9)) // Construction du systeme Ax=B @@ -305,8 +295,7 @@ void vpMeLine::leastSquare() { nos_1 = numberOfSignal(); unsigned int k = 0; - for (std::list::const_iterator it = list.begin(); - it != list.end(); ++it) { + for (std::list::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; @@ -336,8 +325,7 @@ void vpMeLine::leastSquare() } k = 0; - for (std::list::iterator it = list.begin(); it != list.end(); - ++it) { + for (std::list::iterator it = list.begin(); it != list.end(); ++it) { p_me = *it; if (p_me.getState() == vpMeSite::NO_SUPPRESSION) { if (w[k] < 0.2) { @@ -366,8 +354,7 @@ void vpMeLine::leastSquare() { nos_1 = numberOfSignal(); unsigned int k = 0; - for (std::list::const_iterator it = list.begin(); - it != list.end(); ++it) { + for (std::list::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; @@ -397,8 +384,7 @@ void vpMeLine::leastSquare() } k = 0; - for (std::list::iterator it = list.begin(); it != list.end(); - ++it) { + for (std::list::iterator it = list.begin(); it != list.end(); ++it) { p_me = *it; if (p_me.getState() == vpMeSite::NO_SUPPRESSION) { if (w[k] < 0.2) { @@ -434,8 +420,7 @@ void vpMeLine::leastSquare() \param ip1 : Coordinates of the first point. \param ip2 : Coordinates of the second point. */ -void vpMeLine::initTracking(const vpImage &I, - const vpImagePoint &ip1, const vpImagePoint &ip2) +void vpMeLine::initTracking(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2) { vpCDEBUG(1) << " begin vpMeLine::initTracking()" << std::endl; @@ -512,8 +497,7 @@ void vpMeLine::setExtremities() double jmax = -1; // Loop through list of sites to track - for (std::list::const_iterator it = list.begin(); - it != list.end(); ++it) { + for (std::list::const_iterator it = list.begin(); it != list.end(); ++it) { vpMeSite s = *it; // current reference pixel if (s.ifloat < imin) { imin = s.ifloat; @@ -532,8 +516,7 @@ void vpMeLine::setExtremities() PExt[1].jfloat = jmax; if (fabs(imin - imax) < 25) { - for (std::list::const_iterator it = list.begin(); - it != list.end(); ++it) { + for (std::list::const_iterator it = list.begin(); it != list.end(); ++it) { vpMeSite s = *it; // current reference pixel if (s.jfloat < jmin) { imin = s.ifloat; @@ -569,8 +552,7 @@ void vpMeLine::seekExtremities(const vpImage &I) if (!me) { vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized"); - throw(vpTrackingException(vpTrackingException::initializationError, - "Moving edges not initialized")); + throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized")); } int rows = (int)I.getHeight(); @@ -578,12 +560,10 @@ void vpMeLine::seekExtremities(const vpImage &I) double n_sample; // if (me->getSampleStep()==0) - if (std::fabs(me->getSampleStep()) <= - std::numeric_limits::epsilon()) { + if (std::fabs(me->getSampleStep()) <= std::numeric_limits::epsilon()) { vpERROR_TRACE("function called with sample step = 0"); - throw(vpTrackingException(vpTrackingException::fatalError, - "sample step = 0")); + throw(vpTrackingException(vpTrackingException::fatalError, "sample step = 0")); } // i, j portions of the line_p @@ -688,8 +668,7 @@ void vpMeLine::reSample(const vpImage &I) if (!me) { vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized"); - throw(vpTrackingException(vpTrackingException::initializationError, - "Moving edges not initialized")); + throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized")); } project(a, b, c, PExt[0].ifloat, PExt[0].jfloat, i1, j1); @@ -737,8 +716,7 @@ void vpMeLine::updateDelta() angle_ = vpMath::round(angle_ * 180 / M_PI); // if(fabs(angle_) == 180 ) - if (std::fabs(std::fabs(angle_) - 180) <= - std::numeric_limits::epsilon()) { + if (std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits::epsilon()) { angle_ = 0; } @@ -749,8 +727,7 @@ void vpMeLine::updateDelta() angle_1 = angle_; - for (std::list::iterator it = list.begin(); it != list.end(); - ++it) { + for (std::list::iterator it = list.begin(); it != list.end(); ++it) { p_me = *it; p_me.alpha = delta; p_me.mask_sign = sign; @@ -825,8 +802,7 @@ void vpMeLine::track(const vpImage &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) +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); @@ -888,8 +864,7 @@ void vpMeLine::computeRhoTheta(const vpImage &I) 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_) { + 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)); @@ -901,8 +876,7 @@ void vpMeLine::computeRhoTheta(const vpImage &I) 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")); + throw(vpTrackingException(vpTrackingException::fatalError, "increment is too small")); } update_indices(theta, i, j, incr, i1, i2, j1, j2); } @@ -919,10 +893,9 @@ void vpMeLine::computeRhoTheta(const vpImage &I) incr--; end = false; if (incr == 1) { - throw(vpException(vpException::fatalError, - "In vpMeLine cannot determine rho sign, since " - "there is no gray level difference between both " - "sides of the line")); + throw(vpException(vpException::fatalError, "In vpMeLine cannot determine rho sign, since " + "there is no gray level difference between both " + "sides of the line")); } } update_indices(theta, i, j, incr, i1, i2, j1, j2); @@ -1013,8 +986,7 @@ void vpMeLine::getExtremities(vpImagePoint &ip1, vpImagePoint &ip2) \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) +bool vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip) { double a1 = line1.a; double b1 = line1.b; @@ -1032,9 +1004,7 @@ bool vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2, // if (denom == 0) if (std::fabs(denom) <= std::numeric_limits::epsilon()) { - std::cout - << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" - << std::endl; + std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl; return (false); } @@ -1050,9 +1020,7 @@ bool vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2, // if (denom == 0) if (std::fabs(denom) <= std::numeric_limits::epsilon()) { - std::cout - << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" - << std::endl; + std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl; return (false); } @@ -1091,10 +1059,8 @@ bool vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2, \param thickness : Thickness of the line. */ -void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, - const vpMeSite &PExt2, const double &A, - const double &B, const double &C, const vpColor &color, - unsigned int thickness) +void vpMeLine::display(const vpImage &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; @@ -1156,10 +1122,8 @@ void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, \param thickness : Thickness of the line. */ -void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, - const vpMeSite &PExt2, const double &A, - const double &B, const double &C, const vpColor &color, - unsigned int thickness) +void vpMeLine::display(const vpImage &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; @@ -1223,16 +1187,13 @@ void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, \param thickness : Thickness of the line. */ -void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, - const vpMeSite &PExt2, - const std::list &site_list, const double &A, - const double &B, const double &C, const vpColor &color, - unsigned int thickness) +void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, + const std::list &site_list, const double &A, const double &B, const double &C, + const vpColor &color, unsigned int thickness) { vpImagePoint ip; - for (std::list::const_iterator it = site_list.begin(); - it != site_list.end(); ++it) { + for (std::list::const_iterator it = site_list.begin(); it != site_list.end(); ++it) { vpMeSite pix = *it; ip.set_i(pix.ifloat); ip.set_j(pix.jfloat); @@ -1307,16 +1268,13 @@ void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, \param thickness : Thickness of the line. */ -void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, - const vpMeSite &PExt2, - const std::list &site_list, const double &A, - const double &B, const double &C, const vpColor &color, - unsigned int thickness) +void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, + const std::list &site_list, const double &A, const double &B, const double &C, + const vpColor &color, unsigned int thickness) { vpImagePoint ip; - for (std::list::const_iterator it = site_list.begin(); - it != site_list.end(); ++it) { + for (std::list::const_iterator it = site_list.begin(); it != site_list.end(); ++it) { vpMeSite pix = *it; ip.set_i(pix.ifloat); ip.set_j(pix.jfloat); diff --git a/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp b/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp index 4793c03ca4..b36f5b7d99 100644 --- a/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp @@ -66,10 +66,8 @@ #endif double computeDelta(double deltai, double deltaj); -void findAngle(const vpImage &I, const vpImagePoint &iP, - vpMe *me, double &angle, double &convlt); -vpImagePoint findFirstBorder(const vpImage &Isub, - const vpImagePoint &iP); +void findAngle(const vpImage &I, const vpImagePoint &iP, vpMe *me, double &angle, double &convlt); +vpImagePoint findFirstBorder(const vpImage &Isub, const vpImagePoint &iP); bool findCenterPoint(std::list *ip_edges_list); #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS vp_deprecated bool findCenterPoint(vpList *ip_edges_list); @@ -95,16 +93,15 @@ double computeDelta(double deltai, double deltaj) // its edge to enable the computation of a convolution whith a mask. static bool outOfImage(const 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))); + 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 &I, const vpImagePoint &iP, - vpMe *me, double &angle, double &convlt) +void findAngle(const vpImage &I, const vpImagePoint &iP, vpMe *me, double &angle, double &convlt) { int Iheight = (int)I.getHeight(); int Iwidth = (int)I.getWidth(); @@ -157,19 +154,16 @@ void findAngle(const vpImage &I, const vpImagePoint &iP, //- 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 &Isub, - const vpImagePoint &iP) +vpImagePoint findFirstBorder(const vpImage &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 (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)); + dist = vpImagePoint::sqrDistance(vpImagePoint(iP), vpImagePoint(i, j)); if (dist <= 16 && dist < dist_1) { dist_1 = dist; index.set_ij(i, j); @@ -203,8 +197,7 @@ vp_deprecated bool findCenterPoint(vpList *ip_edges_list) // pixels from the center of the sub image (ie the point (15,15). bool findCenterPoint(std::list *ip_edges_list) { - for (std::list::const_iterator it = ip_edges_list->begin(); - it != ip_edges_list->end(); ++it) { + for (std::list::const_iterator it = ip_edges_list->begin(); it != ip_edges_list->end(); ++it) { vpImagePoint iP = *it; double dist = vpImagePoint::sqrDistance(iP, vpImagePoint(15, 15)); if (dist <= 16) { @@ -220,8 +213,8 @@ bool findCenterPoint(std::list *ip_edges_list) Basic constructor that calls the constructor of the class vpMeTracker. */ vpMeNurbs::vpMeNurbs() - : nurbs(), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), - enableCannyDetection(false), cannyTh1(100.), cannyTh2(200.) + : nurbs(), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), enableCannyDetection(false), cannyTh1(100.), + cannyTh2(200.) { } @@ -229,9 +222,8 @@ vpMeNurbs::vpMeNurbs() Copy constructor. */ vpMeNurbs::vpMeNurbs(const vpMeNurbs &menurbs) - : vpMeTracker(menurbs), nurbs(menurbs.nurbs), dist(0.), nbControlPoints(20), - beginPtFound(0), endPtFound(0), enableCannyDetection(false), - cannyTh1(100.), cannyTh2(200.) + : vpMeTracker(menurbs), nurbs(menurbs.nurbs), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), + enableCannyDetection(false), cannyTh1(100.), cannyTh2(200.) { dist = menurbs.dist; nbControlPoints = menurbs.nbControlPoints; @@ -272,8 +264,7 @@ void vpMeNurbs::initTracking(const vpImage &I) if (ptList.size() > 3) initTracking(I, ptList); else - throw(vpException(vpException::notInitialized, - "Not enough points to initialize the Nurbs")); + throw(vpException(vpException::notInitialized, "Not enough points to initialize the Nurbs")); } /*! @@ -283,8 +274,7 @@ void vpMeNurbs::initTracking(const vpImage &I) \param I : Image in which the edge appears. \param ptList : List of point to initialize the Nurbs. */ -void vpMeNurbs::initTracking(const vpImage &I, - const std::list &ptList) +void vpMeNurbs::initTracking(const vpImage &I, const std::list &ptList) { nurbs.globalCurveInterp(ptList); @@ -321,8 +311,7 @@ void vpMeNurbs::sample(const vpImage &I) // 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())) { + 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); @@ -418,13 +407,11 @@ void vpMeNurbs::seekExtremities(const vpImage &I) double threshold = 3 * me->getSampleStep(); double sample_step = me->getSampleStep(); vpImagePoint pt; - if (d > - threshold /*|| (list.firstValue()).mask_sign != (list.lastValue()).mask_sign*/) { + 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.init(begin[0].get_i(), begin[0].get_j(), (list.front()).alpha, 0, (list.front()).mask_sign); P.setDisplay(selectDisplay); // Set the range @@ -468,8 +455,7 @@ void vpMeNurbs::seekExtremities(const vpImage &I) if (!beginPtAdded) beginPtFound++; - P.init(end[0].get_i(), end[0].get_j(), (list.back()).alpha, 0, - (list.back()).mask_sign); + P.init(end[0].get_i(), end[0].get_j(), (list.back()).alpha, 0, (list.back()).mask_sign); P.setDisplay(selectDisplay); bool endPtAdded = false; @@ -565,8 +551,7 @@ void vpMeNurbs::seekExtremitiesCanny(const vpImage & /* I */) IplImage *Ip = NULL; vpImageConvert::convert(Isub, Ip); - IplImage *dst = cvCreateImage( - cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1); + IplImage *dst = cvCreateImage(cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1); cvCanny(Ip, dst, cannyTh1, cannyTh2, 3); vpImageConvert::convert(dst, Isub); @@ -587,17 +572,13 @@ void vpMeNurbs::seekExtremitiesCanny(const vpImage & /* I */) if (std::fabs(fi) <= std::numeric_limits::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::epsilon()) + else if (std::fabs(fi - h) <= std::fabs(vpMath::maximum(fi, h)) * std::numeric_limits::epsilon()) dir = 0; // else if (firstBorder.get_j() == 0) dir = 2; else if (std::fabs(fj) <= std::numeric_limits::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::epsilon()) + else if (std::fabs(fj - w) <= std::fabs(vpMath::maximum(fj, w)) * std::numeric_limits::epsilon()) dir = 6; computeFreemanChainElement(Isub, firstBorder, dir); unsigned int firstDir = dir; @@ -612,8 +593,7 @@ void vpMeNurbs::seekExtremitiesCanny(const vpImage & /* I */) ip_edges_list.push_back(border); computeFreemanChainElement(Isub, border, dir); - } while ((border != firstBorder || dir != firstDir) && - isInImage(Isub, border)); + } while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border)); } if (findCenterPoint(&ip_edges_list)) { @@ -632,9 +612,8 @@ void vpMeNurbs::seekExtremitiesCanny(const vpImage & /* I */) double delta = 0; int nbr = 0; std::list addedPt; - for (std::list::const_iterator itEdges = - ip_edges_list.begin(); - itEdges != ip_edges_list.end(); ++itEdges) { + for (std::list::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end(); + ++itEdges) { vpMeSite s = *itList; vpImagePoint iPtemp = *itEdges + topLeft; vpMeSite pix; @@ -642,8 +621,7 @@ void vpMeNurbs::seekExtremitiesCanny(const vpImage & /* I */) dist = vpMeSite::sqrDistance(s, pix); if (dist >= vpMath::sqr(me->getSampleStep()) /*25*/) { bool exist = false; - for (std::list::const_iterator itAdd = addedPt.begin(); - itAdd != addedPt.end(); ++itAdd) { + for (std::list::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) { dist = vpMeSite::sqrDistance(pix, *itAdd); if (dist < vpMath::sqr(me->getSampleStep()) /*25*/) exist = true; @@ -709,8 +687,7 @@ void vpMeNurbs::seekExtremitiesCanny(const vpImage & /* I */) IplImage *Ip = NULL; vpImageConvert::convert(Isub, Ip); - IplImage *dst = cvCreateImage( - cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1); + IplImage *dst = cvCreateImage(cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1); cvCanny(Ip, dst, cannyTh1, cannyTh2, 3); vpImageConvert::convert(dst, Isub); @@ -731,17 +708,13 @@ void vpMeNurbs::seekExtremitiesCanny(const vpImage & /* I */) if (std::fabs(fi) <= std::numeric_limits::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::epsilon()) + else if (std::fabs(fi - h) <= std::fabs(vpMath::maximum(fi, h)) * std::numeric_limits::epsilon()) dir = 0; // else if (firstBorder.get_j() == 0) dir = 2; else if (std::fabs(fj) <= std::numeric_limits::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::epsilon()) + else if (std::fabs(fj - w) <= std::fabs(vpMath::maximum(fj, w)) * std::numeric_limits::epsilon()) dir = 6; computeFreemanChainElement(Isub, firstBorder, dir); @@ -757,8 +730,7 @@ void vpMeNurbs::seekExtremitiesCanny(const vpImage & /* I */) ip_edges_list.push_back(border); computeFreemanChainElement(Isub, border, dir); - } while ((border != firstBorder || dir != firstDir) && - isInImage(Isub, border)); + } while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border)); } if (findCenterPoint(&ip_edges_list)) { @@ -781,9 +753,8 @@ void vpMeNurbs::seekExtremitiesCanny(const vpImage & /* I */) double delta; int nbr = 0; std::list addedPt; - for (std::list::const_iterator itEdges = - ip_edges_list.begin(); - itEdges != ip_edges_list.end(); ++itEdges) { + for (std::list::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end(); + ++itEdges) { s = *itList; vpImagePoint iPtemp = *itEdges + topLeft; vpMeSite pix; @@ -791,8 +762,7 @@ void vpMeNurbs::seekExtremitiesCanny(const vpImage & /* I */) dist = vpMeSite::sqrDistance(s, pix); if (dist >= vpMath::sqr(me->getSampleStep())) { bool exist = false; - for (std::list::const_iterator itAdd = addedPt.begin(); - itAdd != addedPt.end(); ++itAdd) { + for (std::list::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) { dist = vpMeSite::sqrDistance(pix, *itAdd); if (dist < vpMath::sqr(me->getSampleStep())) exist = true; @@ -890,10 +860,8 @@ void vpMeNurbs::localReSample(const vpImage &I) 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); + double dmin1 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iP0); + double dmin2 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iPend); if (dmin1 < dmin1_1) { dmin1_1 = dmin1; @@ -908,24 +876,18 @@ void vpMeNurbs::localReSample(const vpImage &I) 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::epsilon()) || - (std::fabs(uend - 1.0) > - std::fabs(vpMath::maximum(uend, 1.0)) * - std::numeric_limits::epsilon())) { + if ((std::fabs(u - 1.0) > std::fabs(vpMath::maximum(u, 1.0)) * std::numeric_limits::epsilon()) || + (std::fabs(uend - 1.0) > std::fabs(vpMath::maximum(uend, 1.0)) * std::numeric_limits::epsilon())) { iP = nurbs.computeCurveDersPoint(u, 1); - while (vpImagePoint::sqrDistance(iP[0], iPend) > - vpMath::sqr(me->getSampleStep()) && - u < uend) { + 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()) && + 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(); @@ -1018,8 +980,7 @@ void vpMeNurbs::track(const vpImage &I) suppressPoints(); if (list.size() == 1) - throw(vpTrackingException(vpTrackingException::notEnoughPointError, - "Not enough valid me to track")); + throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Not enough valid me to track")); // Recalcule les parametres // nurbs.globalCurveInterp(list); @@ -1063,10 +1024,7 @@ void vpMeNurbs::track(const vpImage &I) \param col : Color of the displayed line. */ -void vpMeNurbs::display(const vpImage &I, vpColor col) -{ - vpMeNurbs::display(I, nurbs, col); -} +void vpMeNurbs::display(const vpImage &I, vpColor col) { vpMeNurbs::display(I, nurbs, col); } /*! Considering a pixel iP compute the next element of the Freeman chain @@ -1084,9 +1042,7 @@ void vpMeNurbs::display(const vpImage &I, vpColor col) \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 &I, - vpImagePoint &iP, - unsigned int &element) +bool vpMeNurbs::computeFreemanChainElement(const vpImage &I, vpImagePoint &iP, unsigned int &element) { vpImagePoint diP; vpImagePoint iPtemp; @@ -1168,14 +1124,12 @@ bool vpMeNurbs::computeFreemanChainElement(const vpImage &I, \return false : Otherwise */ -bool vpMeNurbs::hasGoodLevel(const vpImage &I, - const vpImagePoint &iP) const +bool vpMeNurbs::hasGoodLevel(const vpImage &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) { + if (I((unsigned int)vpMath::round(iP.get_i()), (unsigned int)vpMath::round(iP.get_j())) > 0) { return true; } else { return false; @@ -1192,11 +1146,9 @@ bool vpMeNurbs::hasGoodLevel(const vpImage &I, \return true if the image point \e iP is in the image and false otherwise. */ -bool vpMeNurbs::isInImage(const vpImage &I, - const vpImagePoint &iP) const +bool vpMeNurbs::isInImage(const vpImage &I, const vpImagePoint &iP) const { - return (iP.get_i() >= 0 && iP.get_j() >= 0 && iP.get_i() < I.getHeight() && - iP.get_j() < I.getWidth()); + return (iP.get_i() >= 0 && iP.get_j() >= 0 && iP.get_i() < I.getHeight() && iP.get_j() < I.getWidth()); } /*! @@ -1216,8 +1168,7 @@ bool vpMeNurbs::isInImage(const vpImage &I, 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) +void vpMeNurbs::computeFreemanParameters(unsigned int element, vpImagePoint &diP) { /* 5 6 7 @@ -1272,13 +1223,11 @@ void vpMeNurbs::computeFreemanParameters(unsigned int element, \return true if the point iP is at least 20 pixels far from the image edeges. */ -bool vpMeNurbs::farFromImageEdge(const vpImage &I, - const vpImagePoint &iP) +bool vpMeNurbs::farFromImageEdge(const vpImage &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); + return (iP.get_i() < height - 20 && iP.get_j() < width - 20 && iP.get_i() > 20 && iP.get_j() > 20); } /*! @@ -1291,8 +1240,7 @@ bool vpMeNurbs::farFromImageEdge(const vpImage &I, \param color : Color used to display the nurbs. */ -void vpMeNurbs::display(const vpImage &I, vpNurbs &n, - vpColor color) +void vpMeNurbs::display(const vpImage &I, vpNurbs &n, vpColor color) { double u = 0.0; vpImagePoint pt; diff --git a/modules/tracker/me/src/moving-edges/vpMeSite.cpp b/modules/tracker/me/src/moving-edges/vpMeSite.cpp index b062ed1c7b..276d2f5ffc 100644 --- a/modules/tracker/me/src/moving-edges/vpMeSite.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeSite.cpp @@ -59,8 +59,7 @@ static bool horsImage(int i, int j, int half, int rows, int cols) 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)); + return ((0 < (half_1 - i)) || ((i - rows + half_3) > 0) || (0 < (half_1 - j)) || ((j - cols + half_3) > 0)); } #endif @@ -94,9 +93,8 @@ void vpMeSite::init() } 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) + : 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) @@ -105,9 +103,8 @@ vpMeSite::vpMeSite() } 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) + : 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) @@ -123,9 +120,8 @@ vpMeSite::vpMeSite(double ip, double 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) + : 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) @@ -170,8 +166,7 @@ void vpMeSite::init(double ip, double jp, double alphap, double convltp) j_1 = 0; } // initialise with convolution and sign -void vpMeSite::init(double ip, double jp, double alphap, double convltp, - int sign) +void vpMeSite::init(double ip, double jp, double alphap, double convltp, int sign) { selectDisplay = NONE; ifloat = ip; @@ -221,8 +216,7 @@ vpMeSite &vpMeSite::operator=(const vpMeSite &m) */ // =================================================================== -vpMeSite *vpMeSite::getQueryList(const vpImage &I, - const int range) +vpMeSite *vpMeSite::getQueryList(const vpImage &I, const int range) { int k; @@ -287,17 +281,13 @@ void vpMeSite::getSign(const vpImage &I, const int range) // First extremity k = -range; - unsigned int i1 = - static_cast(vpMath::round(ifloat + k * salpha)); - unsigned int j1 = - static_cast(vpMath::round(jfloat + k * calpha)); + unsigned int i1 = static_cast(vpMath::round(ifloat + k * salpha)); + unsigned int j1 = static_cast(vpMath::round(jfloat + k * calpha)); // Second extremity k = range; - unsigned int i2 = - static_cast(vpMath::round(ifloat + k * salpha)); - unsigned int j2 = - static_cast(vpMath::round(jfloat + k * calpha)); + unsigned int i2 = static_cast(vpMath::round(ifloat + k * salpha)); + unsigned int j2 = static_cast(vpMath::round(jfloat + k * calpha)); // TODO: Here check if i1,j1,i2,j2 > 0 else ?? if (I[i1][j1] > I[i2][j2]) @@ -339,8 +329,7 @@ double vpMeSite::convolution(const vpImage &I, const vpMe *me) thetadeg = 0; } - unsigned int index_mask = - (unsigned int)(thetadeg / (double)me->getAngleStep()); + unsigned int index_mask = (unsigned int)(thetadeg / (double)me->getAngleStep()); unsigned int i_ = static_cast(i); unsigned int j_ = static_cast(j); @@ -370,8 +359,7 @@ double vpMeSite::convolution(const vpImage &I, const vpMe *me) is needed. */ -void vpMeSite::track(const vpImage &I, const vpMe *me, - const bool test_contraste) +void vpMeSite::track(const vpImage &I, const vpMe *me, const bool test_contraste) { // vpMeSite *list_query_pixels ; // int max_rank =0 ; @@ -542,8 +530,7 @@ void vpMeSite::track(const vpImage &I, const vpMe *me, likelihood[n] = fabs(convolution_ + convlt); if (likelihood[n] > threshold) { contraste = convolution_ / convlt; - if ((contraste > contraste_min) && (contraste < contraste_max) && - fabs(1 - contraste) < diff) { + if ((contraste > contraste_min) && (contraste < contraste_max) && fabs(1 - contraste) < diff) { diff = fabs(1 - contraste); max_convolution = convolution_; max = likelihood[n]; @@ -607,26 +594,19 @@ void vpMeSite::track(const vpImage &I, const vpMe *me, } } -int vpMeSite::operator!=(const vpMeSite &m) -{ - return ((m.i != i) || (m.j != j)); -} +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 + 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 &I) -{ - vpMeSite::display(I, ifloat, jfloat, state); -} +void vpMeSite::display(const vpImage &I) { vpMeSite::display(I, ifloat, jfloat, state); } // Static functions @@ -648,8 +628,7 @@ void vpMeSite::display(const vpImage &I) \param j : Pixel j of the site \param state : state of the site */ -void vpMeSite::display(const vpImage &I, const double &i, - const double &j, const vpMeSiteState &state) +void vpMeSite::display(const vpImage &I, const double &i, const double &j, const vpMeSiteState &state) { switch (state) { case NO_SUPPRESSION: @@ -695,8 +674,7 @@ void vpMeSite::display(const vpImage &I, const double &i, \param j : Pixel j of the site \param state : state of the site */ -void vpMeSite::display(const vpImage &I, const double &i, - const double &j, const vpMeSiteState &state) +void vpMeSite::display(const vpImage &I, const double &i, const double &j, const vpMeSiteState &state) { switch (state) { case NO_SUPPRESSION: diff --git a/modules/tracker/me/src/moving-edges/vpMeTracker.cpp b/modules/tracker/me/src/moving-edges/vpMeTracker.cpp index d728ab0190..cf9774a77c 100644 --- a/modules/tracker/me/src/moving-edges/vpMeTracker.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeTracker.cpp @@ -60,8 +60,7 @@ void vpMeTracker::init() } vpMeTracker::vpMeTracker() - : list(), me(NULL), init_range(1), nGoodElement(0), - selectDisplay(vpMeSite::NONE) + : list(), me(NULL), init_range(1), nGoodElement(0), selectDisplay(vpMeSite::NONE) #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS , query_range(0), display_point(false) @@ -71,8 +70,7 @@ vpMeTracker::vpMeTracker() } vpMeTracker::vpMeTracker(const vpMeTracker &meTracker) - : vpTracker(meTracker), list(), me(NULL), init_range(1), nGoodElement(0), - selectDisplay(vpMeSite::NONE) + : vpTracker(meTracker), list(), me(NULL), init_range(1), nGoodElement(0), selectDisplay(vpMeSite::NONE) #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS , query_range(0), display_point(false) @@ -117,39 +115,29 @@ vpMeTracker &vpMeTracker::operator=(vpMeTracker &p_me) return *this; } -static bool isSuppressZero(const vpMeSite &P) -{ - return (P.getState() == vpMeSite::NO_SUPPRESSION); -} +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( - std::count_if(list.begin(), list.end(), isSuppressZero)); + number_signal = static_cast(std::count_if(list.begin(), list.end(), isSuppressZero)); return number_signal; } -unsigned int vpMeTracker::totalNumberOfSignal() -{ - return (unsigned int)list.size(); -} +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)))); + return (!((i > half + 2) && (i < rows - (half + 2)) && (j > half + 2) && (j < cols - (half + 2)))); } -int vpMeTracker::outOfImage(const vpImagePoint &iP, int half, int rows, - int cols) +int vpMeTracker::outOfImage(const 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)))); + return (!((i > half + 2) && (i < rows - (half + 2)) && (j > half + 2) && (j < cols - (half + 2)))); } /*! @@ -163,8 +151,7 @@ void vpMeTracker::initTracking(const vpImage &I) { if (!me) { vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized"); - throw(vpTrackingException(vpTrackingException::initializationError, - "Moving edges not initialized")); + throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized")); } // Must set range to 0 @@ -177,8 +164,7 @@ void vpMeTracker::initTracking(const vpImage &I) vpImagePoint ip1, ip2; // Loop through list of sites to track - for (std::list::iterator it = list.begin(); it != list.end(); - ++it) { + for (std::list::iterator it = list.begin(); it != list.end(); ++it) { vpMeSite refp = *it; // current reference pixel d++; @@ -246,22 +232,19 @@ void vpMeTracker::track(const vpImage &I) { if (!me) { vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized"); - throw(vpTrackingException(vpTrackingException::initializationError, - "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")); + 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::iterator it = list.begin(); it != list.end(); - ++it) { + for (std::list::iterator it = list.begin(); it != list.end(); ++it) { vpMeSite s = *it; // current reference pixel // d++ ; @@ -320,12 +303,10 @@ void vpMeTracker::display(const vpImage &I) #if (DEBUG_LEVEL1) { std::cout << "begin vpMeTracker::displayList() " << std::endl; - std::cout << " There are " << list.size() << " sites in the list " - << std::endl; + std::cout << " There are " << list.size() << " sites in the list " << std::endl; } #endif - for (std::list::const_iterator it = list.begin(); - it != list.end(); ++it) { + for (std::list::const_iterator it = list.begin(); it != list.end(); ++it) { vpMeSite p_me = *it; p_me.display(I); } @@ -337,11 +318,9 @@ void vpMeTracker::display(const vpImage &I) \param w : vector \param index_w : index */ -void vpMeTracker::display(const vpImage &I, vpColVector &w, - unsigned int &index_w) +void vpMeTracker::display(const vpImage &I, vpColVector &w, unsigned int &index_w) { - for (std::list::iterator it = list.begin(); it != list.end(); - ++it) { + for (std::list::iterator it = list.begin(); it != list.end(); ++it) { vpMeSite P = *it; if (P.getState() == vpMeSite::NO_SUPPRESSION) { diff --git a/modules/tracker/me/src/moving-edges/vpNurbs.cpp b/modules/tracker/me/src/moving-edges/vpNurbs.cpp index b2432220e2..84091489a8 100644 --- a/modules/tracker/me/src/moving-edges/vpNurbs.cpp +++ b/modules/tracker/me/src/moving-edges/vpNurbs.cpp @@ -43,14 +43,12 @@ /* Compute the distance d = |Pw1-Pw2| */ -inline double distance(const vpImagePoint &iP1, const double w1, - const vpImagePoint &iP2, const double w2) +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)); + return sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew)); } /*! @@ -64,10 +62,7 @@ vpNurbs::vpNurbs() : weights() { p = 3; } /*! Copy constructor. */ -vpNurbs::vpNurbs(const vpNurbs &nurbs) - : vpBSpline(nurbs), weights(nurbs.weights) -{ -} +vpNurbs::vpNurbs(const vpNurbs &nurbs) : vpBSpline(nurbs), weights(nurbs.weights) {} /*! Basic destructor @@ -87,11 +82,8 @@ vpNurbs::~vpNurbs() {} return the coordinates of a point corresponding to the knot \f$ u \f$. */ -vpImagePoint -vpNurbs::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, - std::vector &l_knots, - std::vector &l_controlPoints, - std::vector &l_weights) +vpImagePoint vpNurbs::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector &l_knots, + std::vector &l_controlPoints, std::vector &l_weights) { vpBasisFunction *N = NULL; N = computeBasisFuns(l_u, l_i, l_p, l_knots); @@ -101,10 +93,8 @@ vpNurbs::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, 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]; + 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]; } @@ -136,10 +126,8 @@ vpImagePoint vpNurbs::computeCurvePoint(double u) 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]; + 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]; } @@ -179,10 +167,8 @@ vpImagePoint vpNurbs::computeCurvePoint(double u) coresponds to the coordinates (i,j) of the point and the third column corresponds to the associated weight. */ -vpMatrix vpNurbs::computeCurveDers(double l_u, unsigned int l_i, - unsigned int l_p, unsigned int l_der, - std::vector &l_knots, - std::vector &l_controlPoints, +vpMatrix vpNurbs::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, + std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights) { vpMatrix derivate(l_der + 1, 3); @@ -195,14 +181,9 @@ vpMatrix vpNurbs::computeCurveDers(double l_u, unsigned int l_i, 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]); + 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]); } } @@ -247,14 +228,9 @@ vpMatrix vpNurbs::computeCurveDers(double u, unsigned int der) 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]); + 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]); } } @@ -281,10 +257,9 @@ vpMatrix vpNurbs::computeCurveDers(double u, unsigned int der) \f$ for \f$ k = 0, ... , l_{der} \f$. The kth derivative is in the kth cell of the array. */ -vpImagePoint *vpNurbs::computeCurveDersPoint( - double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, - std::vector &l_knots, std::vector &l_controlPoints, - std::vector &l_weights) +vpImagePoint *vpNurbs::computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, + std::vector &l_knots, std::vector &l_controlPoints, + std::vector &l_weights) { std::vector A; vpImagePoint pt; @@ -295,8 +270,7 @@ vpImagePoint *vpNurbs::computeCurveDersPoint( A.push_back(pt); } - vpMatrix Awders = - computeCurveDers(l_u, l_i, l_p, l_der, l_knots, A, l_weights); + vpMatrix Awders = computeCurveDers(l_u, l_i, l_p, l_der, l_knots, A, l_weights); vpImagePoint *CK = new vpImagePoint[l_der + 1]; @@ -348,10 +322,8 @@ vpImagePoint *vpNurbs::computeCurveDersPoint(double u, unsigned int der) \param l_controlPoints : the list of control points. \param l_weights : the list of weights. */ -void vpNurbs::curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, - unsigned int l_r, unsigned int l_p, - std::vector &l_knots, - std::vector &l_controlPoints, +void vpNurbs::curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p, + std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights) { vpMatrix Rw(l_p + 1, 3); @@ -361,10 +333,8 @@ void vpNurbs::curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, 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][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]; } @@ -379,8 +349,7 @@ void vpNurbs::curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, 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]); + 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]; @@ -390,8 +359,7 @@ void vpNurbs::curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, 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]); + 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]; } @@ -436,11 +404,8 @@ void vpNurbs::curveKnotIns(double u, unsigned int s, unsigned int r) \param l_controlPoints : the list of control points. \param l_weights : the list of weights. */ -void vpNurbs::refineKnotVectCurve(double *l_x, unsigned int l_r, - unsigned int l_p, - std::vector &l_knots, - std::vector &l_controlPoints, - std::vector &l_weights) +void vpNurbs::refineKnotVectCurve(double *l_x, unsigned int l_r, unsigned int l_p, std::vector &l_knots, + std::vector &l_controlPoints, std::vector &l_weights) { unsigned int a = findSpan(l_x[0], l_p, l_knots); unsigned int b = findSpan(l_x[l_r], l_p, l_knots); @@ -450,8 +415,7 @@ void vpNurbs::refineKnotVectCurve(double *l_x, unsigned int l_r, 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]); + l_controlPoints[j].set_ij(l_controlPoints[j].get_i() * l_weights[j], l_controlPoints[j].get_j() * l_weights[j]); } std::vector l_knots_tmp(l_knots); @@ -502,14 +466,11 @@ void vpNurbs::refineKnotVectCurve(double *l_x, unsigned int l_r, 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_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]; @@ -518,8 +479,7 @@ void vpNurbs::refineKnotVectCurve(double *l_x, unsigned int l_r, } 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]); + l_controlPoints[j].set_ij(l_controlPoints[j].get_i() / l_weights[j], l_controlPoints[j].get_j() / l_weights[j]); } } @@ -563,19 +523,15 @@ void vpNurbs::refineKnotVectCurve(double *x, unsigned int r) |P|_{max} \f$ is the maximum distance of any point on the original curve from the origin and \f$ d \f$ is the desired bound on deviation. */ -unsigned int -vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, - double l_TOL, unsigned int l_s, unsigned int l_p, - std::vector &l_knots, - std::vector &l_controlPoints, - std::vector &l_weights) +unsigned int vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL, unsigned int l_s, + unsigned int l_p, std::vector &l_knots, + std::vector &l_controlPoints, std::vector &l_weights) { 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]); + 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; @@ -605,22 +561,12 @@ vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, 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); + 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)); + 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--; @@ -632,22 +578,17 @@ vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, 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)); + 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()); + 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)); + 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; } @@ -697,8 +638,7 @@ vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, } 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]); + 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; @@ -725,8 +665,7 @@ vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, |P|_{max} \f$ is the maximum distance of any point on the original curve from the origin and \f$ d \f$ is the desired bound on deviation. */ -unsigned int vpNurbs::removeCurveKnot(double u, unsigned int r, - unsigned int num, double TOL) +unsigned int vpNurbs::removeCurveKnot(double u, unsigned int r, unsigned int num, double TOL) { return removeCurveKnot(u, r, num, TOL, 0, p, knots, controlPoints, weights); } @@ -743,16 +682,13 @@ unsigned int vpNurbs::removeCurveKnot(double u, unsigned int r, need to be > 0. \param l_knots : The knot vector \param l_controlPoints : the list of control points. \param l_weights : the list of weights. */ -void vpNurbs::globalCurveInterp(std::vector &l_crossingPoints, - unsigned int l_p, - std::vector &l_knots, - std::vector &l_controlPoints, +void vpNurbs::globalCurveInterp(std::vector &l_crossingPoints, unsigned int l_p, + std::vector &l_knots, std::vector &l_controlPoints, std::vector &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")); + throw(vpException(vpException::badValue, "Bad degree of the NURBS basis functions")); } l_knots.clear(); @@ -769,9 +705,7 @@ void vpNurbs::globalCurveInterp(std::vector &l_crossingPoints, std::vector 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(ubar[k - 1] + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1) / d); } ubar.push_back(1.0); @@ -866,12 +800,10 @@ void vpNurbs::globalCurveInterp(vpList &l_crossingPoints) \param l_crossingPoints : The list of data points which have to be interpolated. */ -void vpNurbs::globalCurveInterp( - const std::list &l_crossingPoints) +void vpNurbs::globalCurveInterp(const std::list &l_crossingPoints) { std::vector v_crossingPoints; - for (std::list::const_iterator it = l_crossingPoints.begin(); - it != l_crossingPoints.end(); ++it) { + for (std::list::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) { v_crossingPoints.push_back(*it); } globalCurveInterp(v_crossingPoints, p, knots, controlPoints, weights); @@ -913,10 +845,7 @@ void vpNurbs::globalCurveInterp(const std::list &l_crossingPoints) 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); -} +void vpNurbs::globalCurveInterp() { globalCurveInterp(crossingPoints, p, knots, controlPoints, weights); } /*! Method which enables to compute a NURBS curve approximating a set of data @@ -934,10 +863,8 @@ void vpNurbs::globalCurveInterp() l_controlPoints : the list of control points. \param l_weights : the list of weights. */ -void vpNurbs::globalCurveApprox(std::vector &l_crossingPoints, - unsigned int l_p, unsigned int l_n, - std::vector &l_knots, - std::vector &l_controlPoints, +void vpNurbs::globalCurveApprox(std::vector &l_crossingPoints, unsigned int l_p, unsigned int l_n, + std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights) { l_knots.clear(); @@ -953,9 +880,7 @@ void vpNurbs::globalCurveApprox(std::vector &l_crossingPoints, std::vector 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(ubar[k - 1] + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1) / d); ubar.push_back(1.0); // Compute the knot vector @@ -967,8 +892,7 @@ void vpNurbs::globalCurveApprox(std::vector &l_crossingPoints, for (unsigned int j = 1; j <= l_n - l_p; j++) { double i = floor(j * d); double alpha = j * d - i; - l_knots.push_back((1.0 - alpha) * ubar[(unsigned int)i - 1] + - alpha * ubar[(unsigned int)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++) @@ -981,28 +905,22 @@ void vpNurbs::globalCurveApprox(std::vector &l_crossingPoints, 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() - + 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() - + 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()); + 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()); + 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 { @@ -1076,8 +994,7 @@ void vpNurbs::globalCurveApprox(std::vector &l_crossingPoints, \param n : The desired number of control points. This parameter \e n must be under or equal to the number of data points. */ -void vpNurbs::globalCurveApprox(vpList &l_crossingPoints, - unsigned int n) +void vpNurbs::globalCurveApprox(vpList &l_crossingPoints, unsigned int n) { std::vector v_crossingPoints; l_crossingPoints.front(); @@ -1103,12 +1020,10 @@ void vpNurbs::globalCurveApprox(vpList &l_crossingPoints, interpolated. \param n : The desired number of control points. The parameter \e n must be under or equal to the number of data points. */ -void vpNurbs::globalCurveApprox( - const std::list &l_crossingPoints, unsigned int n) +void vpNurbs::globalCurveApprox(const std::list &l_crossingPoints, unsigned int n) { std::vector v_crossingPoints; - for (std::list::const_iterator it = l_crossingPoints.begin(); - it != l_crossingPoints.end(); ++it) { + for (std::list::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) { v_crossingPoints.push_back(*it); } globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights); @@ -1130,12 +1045,10 @@ void vpNurbs::globalCurveApprox( \param n : The desired number of control points. This parameter \e n must be under or equal to the number of data points. */ -void vpNurbs::globalCurveApprox(const std::list &l_crossingPoints, - unsigned int n) +void vpNurbs::globalCurveApprox(const std::list &l_crossingPoints, unsigned int n) { std::vector v_crossingPoints; - for (std::list::const_iterator it = l_crossingPoints.begin(); - it != l_crossingPoints.end(); ++it) { + for (std::list::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) { vpImagePoint pt(it->ifloat, it->jfloat); v_crossingPoints.push_back(pt); } diff --git a/modules/tracker/me/test/testNurbs.cpp b/modules/tracker/me/test/testNurbs.cpp index 18fa7a2d90..896cc5e119 100644 --- a/modules/tracker/me/test/testNurbs.cpp +++ b/modules/tracker/me/test/testNurbs.cpp @@ -65,8 +65,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -112,8 +111,7 @@ OPTIONS: Default\n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -239,50 +237,41 @@ int main(int argc, const char **argv) std::list knots_cur; Nurbs.get_knots(knots_cur); unsigned int i_display = 0; - for (std::list::const_iterator it = knots_cur.begin(); - it != knots_cur.end(); ++it, ++i_display) { + for (std::list::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 controlPoints_cur; Nurbs.get_controlPoints(controlPoints_cur); i_display = 0; - for (std::list::const_iterator - it = controlPoints_cur.begin(); - it != controlPoints_cur.end(); ++it, ++i_display) { + for (std::list::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 weights_cur; Nurbs.get_weights(weights_cur); i_display = 0; - for (std::list::const_iterator it = weights_cur.begin(); - it != weights_cur.end(); ++it, ++i_display) { + for (std::list::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; + 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; + 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; + 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; + 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; @@ -293,8 +282,7 @@ int main(int argc, const char **argv) vpDisplay::displayCross(I, pt, 4, vpColor::red); u += 0.01; } - for (std::list::const_iterator it = controlPoints.begin(); - it != controlPoints.end(); ++it) { + for (std::list::const_iterator it = controlPoints.begin(); it != controlPoints.end(); ++it) { vpDisplay::displayCross(I, *it, 4, vpColor::green); } @@ -324,8 +312,7 @@ int main(int argc, const char **argv) u += 0.01; } - for (std::list::const_iterator it = controlPoints.begin(); - it != controlPoints.end(); ++it) { + for (std::list::const_iterator it = controlPoints.begin(); it != controlPoints.end(); ++it) { vpDisplay::displayCross(I2, *it, 4, vpColor::green); } vpDisplay::flush(I2); @@ -354,8 +341,7 @@ int main(int argc, const char **argv) u += 0.01; } - for (std::list::const_iterator it = controlPoints.begin(); - it != controlPoints.end(); ++it) { + for (std::list::const_iterator it = controlPoints.begin(); it != controlPoints.end(); ++it) { vpDisplay::displayCross(I3, *it, 4, vpColor::green); } @@ -382,8 +368,7 @@ int main(int argc, const char **argv) int main() { std::cout << "This example requires a video device. " << std::endl - << "You should install X11, GTK, OpenCV, GDI or Direct3D" - << std::endl + << "You should install X11, GTK, OpenCV, GDI or Direct3D" << std::endl << "to be able to execute this example." << std::endl; return 0; } diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTracker.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTracker.h index bae445a087..8795711a8b 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTracker.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTracker.h @@ -78,7 +78,7 @@ class VISP_EXPORT vpTemplateTracker unsigned int templateSelectSize; #ifndef DOXYGEN_SHOULD_SKIP_THIS - vpTemplateTrackerPointSuppMIInv *ptTemplateSupp; // pour inverse et compo + vpTemplateTrackerPointSuppMIInv *ptTemplateSupp; // pour inverse et compo vpTemplateTrackerPointSuppMIInv **ptTemplateSuppPyr; // pour inverse et // compo #endif @@ -173,31 +173,23 @@ class VISP_EXPORT vpTemplateTracker 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_() + : 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_() { } explicit vpTemplateTracker(vpTemplateTrackerWarp *_warp); virtual ~vpTemplateTracker(); - void display(const vpImage &I, - const vpColor &col = vpColor::green, - const unsigned int thickness = 3); - void display(const vpImage &I, const vpColor &col = vpColor::green, - const unsigned int thickness = 3); + void display(const vpImage &I, const vpColor &col = vpColor::green, const unsigned int thickness = 3); + void display(const vpImage &I, const vpColor &col = vpColor::green, const unsigned int thickness = 3); bool getDiverge() const { return diverge; } vpColVector getdp() { return dp; } @@ -220,11 +212,8 @@ class VISP_EXPORT vpTemplateTracker vpTemplateTrackerZone getZoneRef() const { return zoneRef_; } void initClick(const vpImage &I, bool delaunay = false); - void initFromPoints(const vpImage &I, - const std::vector &v_ip, - bool delaunay = false); - void initFromZone(const vpImage &I, - const vpTemplateTrackerZone &zone); + void initFromPoints(const vpImage &I, const std::vector &v_ip, bool delaunay = false); + void initFromZone(const vpImage &I, const vpTemplateTrackerZone &zone); void resetTracker(); @@ -269,8 +258,7 @@ class VISP_EXPORT vpTemplateTracker 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 + 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; @@ -289,10 +277,7 @@ class VISP_EXPORT vpTemplateTracker mod_i = sample_i; mod_j = sample_j; } - void setThresholdGradient(double threshold) - { - thresholdGradient = threshold; - } + void setThresholdGradient(double threshold) { thresholdGradient = threshold; } /*! By default Brent usage is disabled. */ void setUseBrent(bool b) { useBrent = b; } @@ -300,22 +285,15 @@ class VISP_EXPORT vpTemplateTracker void trackRobust(const vpImage &I); protected: - void computeOptimalBrentGain(const vpImage &I, - vpColVector &tp, double tMI, - vpColVector &direction, double &alpha); - virtual double getCost(const vpImage &I, - const vpColVector &tp) = 0; - void getGaussianBluredImage(const vpImage &I) - { - vpImageFilter::filter(I, BI, fgG, taillef); - } + void computeOptimalBrentGain(const vpImage &I, vpColVector &tp, double tMI, vpColVector &direction, + double &alpha); + virtual double getCost(const vpImage &I, const vpColVector &tp) = 0; + void getGaussianBluredImage(const vpImage &I) { vpImageFilter::filter(I, BI, fgG, taillef); } virtual void initHessienDesired(const vpImage &I) = 0; virtual void initHessienDesiredPyr(const vpImage &I); virtual void initPyramidal(unsigned int nbLvl, unsigned int l0); - void initTracking(const vpImage &I, - vpTemplateTrackerZone &zone); - virtual void initTrackingPyr(const vpImage &I, - vpTemplateTrackerZone &zone); + void initTracking(const vpImage &I, vpTemplateTrackerZone &zone); + virtual void initTrackingPyr(const vpImage &I, vpTemplateTrackerZone &zone); virtual void trackNoPyr(const vpImage &I) = 0; virtual void trackPyr(const vpImage &I); }; diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerBSpline.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerBSpline.h index d2d76baa58..cfef5f222d 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerBSpline.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerBSpline.h @@ -56,8 +56,7 @@ class VISP_EXPORT vpTemplateTrackerBSpline public: static double Bspline4(double diff); - static double getSubPixBspline4(const vpImage &I, double r, - double t); + static double getSubPixBspline4(const vpImage &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 index a0a24fbc93..f86352e77a 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerHeader.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerHeader.h @@ -76,10 +76,7 @@ struct vpTemplateTrackerPoint { double *dW; double *HiG; - vpTemplateTrackerPoint() - : x(0), y(0), dx(0), dy(0), val(0), dW(NULL), HiG(NULL) - { - } + vpTemplateTrackerPoint() : x(0), y(0), dx(0), dy(0), val(0), dW(NULL), HiG(NULL) {} }; /*! \struct vpTemplateTrackerPointCompo @@ -100,9 +97,7 @@ struct vpTemplateTrackerPointSuppMIInv { double *d2W; double *d2Wx; double *d2Wy; - vpTemplateTrackerPointSuppMIInv() - : et(0), ct(0), BtInit(NULL), Bt(NULL), dBt(NULL), d2W(NULL), d2Wx(NULL), - d2Wy(NULL) + vpTemplateTrackerPointSuppMIInv() : et(0), ct(0), BtInit(NULL), Bt(NULL), dBt(NULL), d2W(NULL), d2Wx(NULL), d2Wy(NULL) { } }; diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardAdditional.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardAdditional.h index 7891b995b7..9b60e2e21a 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardAdditional.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardAdditional.h @@ -52,17 +52,11 @@ The algorithm implemented in this class is described in \cite Baker04a and \cite Marchand16a. */ -class VISP_EXPORT vpTemplateTrackerSSDForwardAdditional - : public vpTemplateTrackerSSD +class VISP_EXPORT vpTemplateTrackerSSDForwardAdditional : public vpTemplateTrackerSSD { public: /*! Minimization method. */ - typedef enum { - USE_NEWTON, - USE_LMA, - USE_GRADIENT, - USE_QUASINEWTON - } vpMinimizationTypeSSDForwardAdditional; + typedef enum { USE_NEWTON, USE_LMA, USE_GRADIENT, USE_QUASINEWTON } vpMinimizationTypeSSDForwardAdditional; private: vpMinimizationTypeSSDForwardAdditional minimizationMethod; @@ -78,9 +72,6 @@ class VISP_EXPORT vpTemplateTrackerSSDForwardAdditional public: explicit vpTemplateTrackerSSDForwardAdditional(vpTemplateTrackerWarp *warp); - void setMinimizationMethod(vpMinimizationTypeSSDForwardAdditional method) - { - minimizationMethod = method; - } + 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 index ac5525a37e..4848f4bd46 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardCompositional.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardCompositional.h @@ -52,8 +52,7 @@ The algorithm implemented in this class is described in \cite Baker04a and \cite Marchand16a. */ -class VISP_EXPORT vpTemplateTrackerSSDForwardCompositional - : public vpTemplateTrackerSSD +class VISP_EXPORT vpTemplateTrackerSSDForwardCompositional : public vpTemplateTrackerSSD { protected: bool compoInitialised; @@ -64,7 +63,6 @@ class VISP_EXPORT vpTemplateTrackerSSDForwardCompositional void trackNoPyr(const vpImage &I); public: - explicit vpTemplateTrackerSSDForwardCompositional( - vpTemplateTrackerWarp *warp); + explicit vpTemplateTrackerSSDForwardCompositional(vpTemplateTrackerWarp *warp); }; #endif diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDInverseCompositional.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDInverseCompositional.h index 236f9075cb..01cba4c25e 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDInverseCompositional.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDInverseCompositional.h @@ -53,8 +53,7 @@ The algorithm implemented in this class is described in \cite Baker04a and \cite Marchand16a. */ -class VISP_EXPORT vpTemplateTrackerSSDInverseCompositional - : public vpTemplateTrackerSSD +class VISP_EXPORT vpTemplateTrackerSSDInverseCompositional : public vpTemplateTrackerSSD { protected: bool compoInitialised; @@ -77,8 +76,7 @@ class VISP_EXPORT vpTemplateTrackerSSDInverseCompositional void initPosEvalRMS(const vpColVector &p); public: - explicit vpTemplateTrackerSSDInverseCompositional( - vpTemplateTrackerWarp *warp); + explicit vpTemplateTrackerSSDInverseCompositional(vpTemplateTrackerWarp *warp); /*! Use only the strong gradient pixels to compute the Jabobian. By default * this feature is disabled. */ diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerTriangle.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerTriangle.h index faab2b3e98..10618daa18 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerTriangle.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerTriangle.h @@ -86,13 +86,10 @@ class VISP_EXPORT vpTemplateTrackerTriangle 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(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); + vpTemplateTrackerTriangle(double x1, double y1, double x2, double y2, double x3, double y3); /*! Return the area of the triangle. @@ -158,10 +155,8 @@ class VISP_EXPORT vpTemplateTrackerTriangle 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(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; diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarp.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarp.h index ca27a6dea2..272a1b83e4 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarp.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarp.h @@ -81,8 +81,7 @@ class VISP_EXPORT vpTemplateTrackerWarp \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; + 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 @@ -95,13 +94,11 @@ class VISP_EXPORT vpTemplateTrackerWarp 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, + 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); + void findWarp(const double *ut0, const double *vt0, const double *u, const double *v, int nb_pt, vpColVector &p); #endif /*! @@ -115,8 +112,7 @@ class VISP_EXPORT vpTemplateTrackerWarp 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; + 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 @@ -135,8 +131,7 @@ class VISP_EXPORT vpTemplateTrackerWarp \param Z : Zone to consider. \param p : Parameters of the warping function. */ - double getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z, - const vpColVector &p); + double getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z, const vpColVector &p); /*! Get the number of parameters of the warping function. @@ -151,8 +146,7 @@ class VISP_EXPORT vpTemplateTrackerWarp \param ParamM : Parameters of the warping function. \param ParamMinv : Inverse parameters. */ - virtual void getParamInverse(const vpColVector &ParamM, - vpColVector &ParamMinv) const = 0; + virtual void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const = 0; /*! Get the parameters of the warping function one level down. @@ -160,8 +154,7 @@ class VISP_EXPORT vpTemplateTrackerWarp \param p : Current parameters of the warping function. \param pdown : Resulting parameters on level down. */ - virtual void getParamPyramidDown(const vpColVector &p, - vpColVector &pdown) = 0; + virtual void getParamPyramidDown(const vpColVector &p, vpColVector &pdown) = 0; /*! Get the parameters of the warping function one level up. @@ -186,8 +179,7 @@ class VISP_EXPORT vpTemplateTrackerWarp \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; + virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const = 0; /*! Set the number of parameters of the warping function. @@ -210,8 +202,7 @@ class VISP_EXPORT vpTemplateTrackerWarp \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); + void warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u, double *v); /*! Warp a point. @@ -222,8 +213,7 @@ class VISP_EXPORT vpTemplateTrackerWarp \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; + virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM) = 0; /*! Warp a point. @@ -232,8 +222,7 @@ class VISP_EXPORT vpTemplateTrackerWarp \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; + virtual void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM) = 0; /*! Inverse Warp a point. @@ -242,8 +231,7 @@ class VISP_EXPORT vpTemplateTrackerWarp \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; + virtual void warpXInv(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM) = 0; /*! Warp a triangle and store the result in a new zone. @@ -253,8 +241,7 @@ class VISP_EXPORT vpTemplateTrackerWarp estimated by the template tracker and returned using vpTemplateTracker::getp(). \param out : Resulting triangle. */ - void warpTriangle(const vpTemplateTrackerTriangle &in, const vpColVector &p, - vpTemplateTrackerTriangle &out); + void warpTriangle(const vpTemplateTrackerTriangle &in, const vpColVector &p, vpTemplateTrackerTriangle &out); /*! Warp a zone and store the result in a new zone. @@ -264,8 +251,7 @@ class VISP_EXPORT vpTemplateTrackerWarp estimated by the template tracker and returned using vpTemplateTracker::getp(). \param out : Resulting zone. */ - void warpZone(const vpTemplateTrackerZone &in, const vpColVector &p, - vpTemplateTrackerZone &out); + 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 index d8a722f361..3f126781c9 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpAffine.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpAffine.h @@ -72,8 +72,7 @@ class VISP_EXPORT vpTemplateTrackerWarpAffine : public vpTemplateTrackerWarp \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); + void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW); /*! Compute the compositionnal derivative of the warping function according to @@ -86,8 +85,7 @@ class VISP_EXPORT vpTemplateTrackerWarpAffine : public vpTemplateTrackerWarp 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, + void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW); /*! @@ -101,8 +99,7 @@ class VISP_EXPORT vpTemplateTrackerWarpAffine : public vpTemplateTrackerWarp 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); + 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 @@ -121,8 +118,7 @@ class VISP_EXPORT vpTemplateTrackerWarpAffine : public vpTemplateTrackerWarp \param ParamM : Parameters of the warping function. \param ParamMinv : Inverse parameters. */ - void getParamInverse(const vpColVector &ParamM, - vpColVector &ParamMinv) const; + void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const; /*! Get the parameters of the warping function one level down. @@ -155,8 +151,7 @@ class VISP_EXPORT vpTemplateTrackerWarpAffine : public vpTemplateTrackerWarp \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; + void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const; /*! Warp a point. @@ -165,8 +160,7 @@ class VISP_EXPORT vpTemplateTrackerWarpAffine : public vpTemplateTrackerWarp \param vXres : Coordinates of the warped point. \param ParamM : Parameters of the warping function. */ - void warpX(const vpColVector &vX, vpColVector &vXres, - const vpColVector &ParamM); + void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM); /*! Warp a point. @@ -177,8 +171,7 @@ class VISP_EXPORT vpTemplateTrackerWarpAffine : public vpTemplateTrackerWarp \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); + void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM); /*! Inverse Warp a point. @@ -187,7 +180,6 @@ class VISP_EXPORT vpTemplateTrackerWarpAffine : public vpTemplateTrackerWarp \param vXres : Coordinates of the warped point. \param ParamM : Parameters of the warping function. */ - void warpXInv(const vpColVector &vX, vpColVector &vXres, - const vpColVector &ParamM); + 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 index 18bc19dc6f..4bd24313d7 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomography.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomography.h @@ -53,8 +53,7 @@ \class vpTemplateTrackerWarpHomography \ingroup group_tt_warp */ -class VISP_EXPORT vpTemplateTrackerWarpHomography - : public vpTemplateTrackerWarp +class VISP_EXPORT vpTemplateTrackerWarpHomography : public vpTemplateTrackerWarp { public: // constructor; @@ -81,8 +80,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomography \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); + void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW); /*! Compute the compositionnal derivative of the warping function according to @@ -95,8 +93,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomography 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, + void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW); /*! @@ -110,8 +107,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomography 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); + 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 @@ -148,8 +144,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomography \param ParamM : Parameters of the warping function. \param ParamMinv : Inverse parameters. */ - void getParamInverse(const vpColVector &ParamM, - vpColVector &ParamMinv) const; + void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const; /*! Get the parameters of the warping function one level down. @@ -182,8 +177,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomography \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; + void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const; /*! Warp a point. @@ -192,8 +186,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomography \param vXres : Coordinates of the warped point. \param ParamM : Parameters of the warping function. */ - void warpX(const vpColVector &vX, vpColVector &vXres, - const vpColVector &ParamM); + void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM); /*! Warp a point. @@ -204,8 +197,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomography \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); + void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM); /*! Inverse Warp a point. @@ -214,7 +206,6 @@ class VISP_EXPORT vpTemplateTrackerWarpHomography \param vXres : Coordinates of the warped point. \param ParamM : Parameters of the warping function. */ - void warpXInv(const vpColVector &vX, vpColVector &vXres, - const vpColVector &ParamM); + 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 index a76657c71a..eb724a36cc 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomographySL3.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomographySL3.h @@ -55,8 +55,7 @@ \class vpTemplateTrackerWarpHomographySL3 \ingroup group_tt_warp */ -class VISP_EXPORT vpTemplateTrackerWarpHomographySL3 - : public vpTemplateTrackerWarp +class VISP_EXPORT vpTemplateTrackerWarpHomographySL3 : public vpTemplateTrackerWarp { protected: vpMatrix G; @@ -93,8 +92,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomographySL3 \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); + void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW); /*! Compute the compositionnal derivative of the warping function according to @@ -107,8 +105,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomographySL3 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, + void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW); /*! @@ -121,8 +118,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomographySL3 \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); + 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 @@ -135,8 +131,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomographySL3 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); + 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 @@ -173,8 +168,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomographySL3 \param ParamM : Parameters of the warping function. \param ParamMinv : Inverse parameters. */ - void getParamInverse(const vpColVector &ParamM, - vpColVector &ParamMinv) const; + void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const; /*! Get the parameters of the warping function one level down. @@ -207,8 +201,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomographySL3 \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; + void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const; /*! Warp a point. @@ -217,8 +210,7 @@ class VISP_EXPORT vpTemplateTrackerWarpHomographySL3 \param vXres : Coordinates of the warped point. \param ParamM : Parameters of the warping function. */ - void warpX(const vpColVector &vX, vpColVector &vXres, - const vpColVector &ParamM); + void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM); /*! Warp a point. @@ -229,14 +221,10 @@ class VISP_EXPORT vpTemplateTrackerWarpHomographySL3 \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); + 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*/) - { - } + 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 index 5d393980f0..88a8694af7 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpRT.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpRT.h @@ -71,8 +71,7 @@ class VISP_EXPORT vpTemplateTrackerWarpRT : public vpTemplateTrackerWarp \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); + void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW); /*! Compute the compositionnal derivative of the warping function according @@ -85,8 +84,7 @@ class VISP_EXPORT vpTemplateTrackerWarpRT : public vpTemplateTrackerWarp 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, + void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW); /*! @@ -100,8 +98,7 @@ class VISP_EXPORT vpTemplateTrackerWarpRT : public vpTemplateTrackerWarp 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); + 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 @@ -120,8 +117,7 @@ class VISP_EXPORT vpTemplateTrackerWarpRT : public vpTemplateTrackerWarp \param ParamM : Parameters of the warping function. \param ParamMinv : Inverse parameters. */ - void getParamInverse(const vpColVector &ParamM, - vpColVector &ParamMinv) const; + void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const; /*! Get the parameters of the warping function one level down. @@ -154,8 +150,7 @@ class VISP_EXPORT vpTemplateTrackerWarpRT : public vpTemplateTrackerWarp \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; + void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const; /*! Warp a point. @@ -164,8 +159,7 @@ class VISP_EXPORT vpTemplateTrackerWarpRT : public vpTemplateTrackerWarp \param vXres : Coordinates of the warped point. \param ParamM : Parameters of the warping function. */ - void warpX(const vpColVector &vX, vpColVector &vXres, - const vpColVector &ParamM); + void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM); /*! Warp a point. @@ -176,8 +170,7 @@ class VISP_EXPORT vpTemplateTrackerWarpRT : public vpTemplateTrackerWarp \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); + void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM); /*! Inverse Warp a point. @@ -186,7 +179,6 @@ class VISP_EXPORT vpTemplateTrackerWarpRT : public vpTemplateTrackerWarp \param vXres : Coordinates of the warped point. \param ParamM : Parameters of the warping function. */ - void warpXInv(const vpColVector &vX, vpColVector &vXres, - const vpColVector &ParamM); + 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 index 68201ed7de..79471aa904 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpSRT.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpSRT.h @@ -71,8 +71,7 @@ class VISP_EXPORT vpTemplateTrackerWarpSRT : public vpTemplateTrackerWarp \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); + void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW); /*! Compute the compositionnal derivative of the warping function according to @@ -85,8 +84,7 @@ class VISP_EXPORT vpTemplateTrackerWarpSRT : public vpTemplateTrackerWarp 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, + void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW); /*! @@ -100,8 +98,7 @@ class VISP_EXPORT vpTemplateTrackerWarpSRT : public vpTemplateTrackerWarp 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); + 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 @@ -120,8 +117,7 @@ class VISP_EXPORT vpTemplateTrackerWarpSRT : public vpTemplateTrackerWarp \param ParamM : Parameters of the warping function. \param ParamMinv : Inverse parameters. */ - void getParamInverse(const vpColVector &ParamM, - vpColVector &ParamMinv) const; + void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const; /*! Get the parameters of the warping function one level down. @@ -154,8 +150,7 @@ class VISP_EXPORT vpTemplateTrackerWarpSRT : public vpTemplateTrackerWarp \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; + void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const; /*! Warp a point. @@ -164,8 +159,7 @@ class VISP_EXPORT vpTemplateTrackerWarpSRT : public vpTemplateTrackerWarp \param vXres : Coordinates of the warped point. \param ParamM : Parameters of the warping function. */ - void warpX(const vpColVector &vX, vpColVector &vXres, - const vpColVector &ParamM); + void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM); /*! Warp a point. @@ -176,8 +170,7 @@ class VISP_EXPORT vpTemplateTrackerWarpSRT : public vpTemplateTrackerWarp \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); + void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM); /*! Inverse Warp a point. @@ -186,7 +179,6 @@ class VISP_EXPORT vpTemplateTrackerWarpSRT : public vpTemplateTrackerWarp \param vXres : Coordinates of the warped point. \param ParamM : Parameters of the warping function. */ - void warpXInv(const vpColVector &vX, vpColVector &vXres, - const vpColVector &ParamM); + 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 index 8f84b0f270..73d2a8440c 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpTranslation.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpTranslation.h @@ -51,8 +51,7 @@ \class vpTemplateTrackerWarpTranslation \ingroup group_tt_warp */ -class VISP_EXPORT vpTemplateTrackerWarpTranslation - : public vpTemplateTrackerWarp +class VISP_EXPORT vpTemplateTrackerWarpTranslation : public vpTemplateTrackerWarp { public: // constructor; @@ -72,8 +71,7 @@ class VISP_EXPORT vpTemplateTrackerWarpTranslation \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); + void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW); /*! Compute the compositionnal derivative of the warping function according to @@ -86,8 +84,7 @@ class VISP_EXPORT vpTemplateTrackerWarpTranslation 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, + void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW); /*! @@ -101,8 +98,7 @@ class VISP_EXPORT vpTemplateTrackerWarpTranslation 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); + 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 @@ -121,8 +117,7 @@ class VISP_EXPORT vpTemplateTrackerWarpTranslation \param ParamM : Parameters of the warping function. \param ParamMinv : Inverse parameters. */ - void getParamInverse(const vpColVector &ParamM, - vpColVector &ParamMinv) const; + void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const; /*! Get the parameters of the warping function one level down. @@ -155,8 +150,7 @@ class VISP_EXPORT vpTemplateTrackerWarpTranslation \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; + void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const; /*! Warp a point. @@ -165,8 +159,7 @@ class VISP_EXPORT vpTemplateTrackerWarpTranslation \param vXres : Coordinates of the warped point. \param ParamM : Parameters of the warping function. */ - void warpX(const vpColVector &vX, vpColVector &vXres, - const vpColVector &ParamM); + void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM); /*! Warp a point. @@ -177,8 +170,7 @@ class VISP_EXPORT vpTemplateTrackerWarpTranslation \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); + void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM); /*! Inverse Warp a point. @@ -187,7 +179,6 @@ class VISP_EXPORT vpTemplateTrackerWarpTranslation \param vXres : Coordinates of the warped point. \param ParamM : Parameters of the warping function. */ - void warpXInv(const vpColVector &vX, vpColVector &vXres, - const vpColVector &ParamM); + void warpXInv(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM); }; #endif diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h index 329b6bc45d..41068d6b59 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h @@ -54,15 +54,13 @@ \cite Marchand16a. */ -class VISP_EXPORT vpTemplateTrackerZNCCForwardAdditional - : public vpTemplateTrackerZNCC +class VISP_EXPORT vpTemplateTrackerZNCCForwardAdditional : public vpTemplateTrackerZNCC { protected: void initHessienDesired(const vpImage &I); void trackNoPyr(const vpImage &I); public: - explicit vpTemplateTrackerZNCCForwardAdditional( - vpTemplateTrackerWarp *warp); + explicit vpTemplateTrackerZNCCForwardAdditional(vpTemplateTrackerWarp *warp); }; #endif diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h index 088e39df79..7e16ed8ae4 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h @@ -55,8 +55,7 @@ The algorithm implemented in this class is described in \cite Irani98a and \cite Marchand16a. */ -class VISP_EXPORT vpTemplateTrackerZNCCInverseCompositional - : public vpTemplateTrackerZNCC +class VISP_EXPORT vpTemplateTrackerZNCCInverseCompositional : public vpTemplateTrackerZNCC { protected: bool compoInitialised; @@ -76,8 +75,7 @@ class VISP_EXPORT vpTemplateTrackerZNCCInverseCompositional void initPosEvalRMS(const vpColVector &p); public: - explicit vpTemplateTrackerZNCCInverseCompositional( - vpTemplateTrackerWarp *warp); + explicit vpTemplateTrackerZNCCInverseCompositional(vpTemplateTrackerWarp *warp); void setThresholdRMS(double threshold) { threshold_RMS = threshold; } }; diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZone.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZone.h index 9a630c3269..ff8cd9d1d2 100644 --- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZone.h +++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZone.h @@ -62,12 +62,11 @@ class VISP_EXPORT vpTemplateTrackerZone { protected: - std::vector - 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 + std::vector 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(); @@ -80,15 +79,11 @@ class VISP_EXPORT vpTemplateTrackerZone void copy(const vpTemplateTrackerZone &z); // display the area on an image - void display(const vpImage &I, - const vpColor &col = vpColor::green, - const unsigned int thickness = 3); - void display(const vpImage &I, const vpColor &col = vpColor::green, - const unsigned int thickness = 3); + void display(const vpImage &I, const vpColor &col = vpColor::green, const unsigned int thickness = 3); + void display(const vpImage &I, const vpColor &col = vpColor::green, const unsigned int thickness = 3); // colorie le tieme triangle - void fillTriangle(vpImage &I, unsigned int id, - unsigned char gray_level); + void fillTriangle(vpImage &I, unsigned int id, unsigned char gray_level); double getArea() const; vpImagePoint getCenter() const; @@ -113,9 +108,7 @@ class VISP_EXPORT vpTemplateTrackerZone // 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 &I, - const std::vector &ip, - bool delaunay = false); + void initFromPoints(const vpImage &I, const std::vector &ip, bool delaunay = false); // check if a point is in the area bool inZone(const int &i, const int &j) const; @@ -123,8 +116,7 @@ class VISP_EXPORT vpTemplateTrackerZone // 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; + bool inZone(const double &i, const double &j, unsigned int &id_triangle) const; vpTemplateTrackerZone &operator=(const vpTemplateTrackerZone &z); }; diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp index 1a3a32b620..e05d348267 100644 --- a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp +++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp @@ -40,8 +40,7 @@ #include -vpTemplateTrackerSSD::vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp) - : vpTemplateTracker(warp), DI(), temp() +vpTemplateTrackerSSD::vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp) : vpTemplateTracker(warp), DI(), temp() { dW.resize(2, nbParam); G.resize(nbParam); @@ -55,8 +54,7 @@ vpTemplateTrackerSSD::vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp) DI.resize(2); } -double vpTemplateTrackerSSD::getCost(const vpImage &I, - const vpColVector &tp) +double vpTemplateTrackerSSD::getCost(const vpImage &I, const vpColVector &tp) { double erreur = 0; double IW; @@ -73,8 +71,7 @@ double vpTemplateTrackerSSD::getCost(const vpImage &I, double j2 = X2[0]; double i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { double Tij = ptTemplate[point].val; if (!blur) IW = I.getValue(i2, j2); @@ -92,8 +89,7 @@ double vpTemplateTrackerSSD::getCost(const vpImage &I, return erreur / Nbpoint; } -double vpTemplateTrackerSSD::getSSD(const vpImage &I, - const vpColVector &tp) +double vpTemplateTrackerSSD::getSSD(const vpImage &I, const vpColVector &tp) { double erreur = 0; double IW; @@ -115,8 +111,7 @@ double vpTemplateTrackerSSD::getSSD(const vpImage &I, double j2 = X2[0]; double i2 = X2[1]; - if ((j2 < I.getWidth() - 1) && (i2 < I.getHeight() - 1) && (i2 > 0) && - (j2 > 0)) { + if ((j2 < I.getWidth() - 1) && (i2 < I.getHeight() - 1) && (i2 > 0) && (j2 > 0)) { double Tij = ptTemplate[point].val; IW = I.getValue(i2, j2); // IW=getSubPixBspline4(I,i2,j2); diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp index e488db80a0..bd5ee7ddb4 100644 --- a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp +++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp @@ -41,8 +41,7 @@ #include vpTemplateTrackerSSDESM::vpTemplateTrackerSSDESM(vpTemplateTrackerWarp *warp) - : vpTemplateTrackerSSD(warp), compoInitialised(false), HDir(), HInv(), - HLMDir(), HLMInv(), GDir(), GInv() + : vpTemplateTrackerSSD(warp), compoInitialised(false), HDir(), HInv(), HLMDir(), HLMInv(), GDir(), GInv() { useCompositionnal = false; useInverse = false; @@ -60,14 +59,9 @@ vpTemplateTrackerSSDESM::vpTemplateTrackerSSDESM(vpTemplateTrackerWarp *warp) GDir.resize(nbParam); } -void vpTemplateTrackerSSDESM::initHessienDesired( - const vpImage &I) -{ - initCompInverse(I); -} +void vpTemplateTrackerSSDESM::initHessienDesired(const vpImage &I) { initCompInverse(I); } -void vpTemplateTrackerSSDESM::initCompInverse( - const vpImage & /*I*/) +void vpTemplateTrackerSSDESM::initCompInverse(const vpImage & /*I*/) { // std::cout<<"Initialise precomputed value of ESM with templateSize: "<< // templateSize<computeDenom(X1, p); ptTemplate[point].dW = new double[nbParam]; - Warp->getdW0(i, j, ptTemplate[point].dy, ptTemplate[point].dx, - ptTemplate[point].dW); + 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++) @@ -138,8 +131,7 @@ void vpTemplateTrackerSSDESM::trackNoPyr(const vpImage &I) j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { // INVERSE Tij = ptTemplate[point].val; if (!blur) @@ -179,8 +171,7 @@ void vpTemplateTrackerSSDESM::trackNoPyr(const vpImage &I) } if (Nbpoint == 0) { // std::cout<<"plus de point dans template suivi"< #include -vpTemplateTrackerSSDForwardAdditional::vpTemplateTrackerSSDForwardAdditional( - vpTemplateTrackerWarp *warp) - : vpTemplateTrackerSSD(warp), minimizationMethod(USE_NEWTON), p_prec(), - G_prec(), KQuasiNewton() +vpTemplateTrackerSSDForwardAdditional::vpTemplateTrackerSSDForwardAdditional(vpTemplateTrackerWarp *warp) + : vpTemplateTrackerSSD(warp), minimizationMethod(USE_NEWTON), p_prec(), G_prec(), KQuasiNewton() { useCompositionnal = false; } -void vpTemplateTrackerSSDForwardAdditional::trackNoPyr( - const vpImage &I) +void vpTemplateTrackerSSDForwardAdditional::trackNoPyr(const vpImage &I) { if (blur) vpImageFilter::filter(I, BI, fgG, taillef); @@ -85,8 +82,7 @@ void vpTemplateTrackerSSDForwardAdditional::trackNoPyr( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Tij = ptTemplate[point].val; if (!blur) @@ -117,8 +113,7 @@ void vpTemplateTrackerSSDForwardAdditional::trackNoPyr( } if (Nbpoint == 0) { // std::cout<<"plus de point dans template suivi"< - std::numeric_limits::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)); + if (std::fabs(s_scal_y) > std::numeric_limits::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; diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardCompositional.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardCompositional.cpp index 730515df7d..34a38df172 100644 --- a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardCompositional.cpp +++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardCompositional.cpp @@ -40,14 +40,12 @@ #include #include -vpTemplateTrackerSSDForwardCompositional:: - vpTemplateTrackerSSDForwardCompositional(vpTemplateTrackerWarp *warp) +vpTemplateTrackerSSDForwardCompositional::vpTemplateTrackerSSDForwardCompositional(vpTemplateTrackerWarp *warp) : vpTemplateTrackerSSD(warp), compoInitialised(false) { } -void vpTemplateTrackerSSDForwardCompositional::initCompo( - const vpImage & /*I*/) +void vpTemplateTrackerSSDForwardCompositional::initCompo(const vpImage & /*I*/) { // std::cout<<"Initialise precomputed value of Compositionnal // Direct"< &I) -{ - initCompo(I); -} +void vpTemplateTrackerSSDForwardCompositional::initHessienDesired(const vpImage &I) { initCompo(I); } -void vpTemplateTrackerSSDForwardCompositional::trackNoPyr( - const vpImage &I) +void vpTemplateTrackerSSDForwardCompositional::trackNoPyr(const vpImage &I) { if (!compoInitialised) std::cout << "Compositionnal tracking no initialised\nUse " @@ -108,8 +101,7 @@ void vpTemplateTrackerSSDForwardCompositional::trackNoPyr( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 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); @@ -144,8 +136,7 @@ void vpTemplateTrackerSSDForwardCompositional::trackNoPyr( } if (Nbpoint == 0) { // std::cout<<"plus de point dans template suivi"< #include -vpTemplateTrackerSSDInverseCompositional:: - vpTemplateTrackerSSDInverseCompositional(vpTemplateTrackerWarp *warp) - : vpTemplateTrackerSSD(warp), compoInitialised(false), HInv(), - HCompInverse(), useTemplateSelect(false), evolRMS(0), x_pos(), y_pos(), - threshold_RMS(1e-8) +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 & /*I*/) +void vpTemplateTrackerSSDInverseCompositional::initCompInverse(const vpImage & /*I*/) { H = 0; @@ -67,8 +64,7 @@ void vpTemplateTrackerSSDInverseCompositional::initCompInverse( Warp->computeDenom(X1, p); ptTemplate[point].dW = new double[nbParam]; - Warp->getdW0(i, j, ptTemplate[point].dy, ptTemplate[point].dx, - ptTemplate[point].dW); + 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++) @@ -102,14 +98,12 @@ void vpTemplateTrackerSSDInverseCompositional::initCompInverse( compoInitialised = true; } -void vpTemplateTrackerSSDInverseCompositional::initHessienDesired( - const vpImage &I) +void vpTemplateTrackerSSDInverseCompositional::initHessienDesired(const vpImage &I) { initCompInverse(I); } -void vpTemplateTrackerSSDInverseCompositional::trackNoPyr( - const vpImage &I) +void vpTemplateTrackerSSDInverseCompositional::trackNoPyr(const vpImage &I) { if (blur) vpImageFilter::filter(I, BI, fgG, taillef); @@ -143,8 +137,7 @@ void vpTemplateTrackerSSDInverseCompositional::trackNoPyr( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Tij = pt->val; if (!blur) IW = I.getValue(i2, j2); @@ -163,8 +156,7 @@ void vpTemplateTrackerSSDInverseCompositional::trackNoPyr( if (Nbpoint == 0) { // std::cout<<"plus de point dans template suivi"< threshold_RMS)); + } while (/*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax) && (evolRMS > threshold_RMS)); nbIteration = iteration; deletePosEvalRMS(); } -void vpTemplateTrackerSSDInverseCompositional::initPosEvalRMS( - const vpColVector &p_) +void vpTemplateTrackerSSDInverseCompositional::initPosEvalRMS(const vpColVector &p_) { unsigned int nb_corners = zoneTracked->getNbTriangle() * 3; x_pos.resize(nb_corners); @@ -211,8 +201,7 @@ void vpTemplateTrackerSSDInverseCompositional::initPosEvalRMS( } } -void vpTemplateTrackerSSDInverseCompositional::computeEvalRMS( - const vpColVector &p_) +void vpTemplateTrackerSSDInverseCompositional::computeEvalRMS(const vpColVector &p_) { unsigned int nb_corners = zoneTracked->getNbTriangle() * 3; diff --git a/modules/tracker/tt/src/tools/vpTemplateTrackerBSpline.cpp b/modules/tracker/tt/src/tools/vpTemplateTrackerBSpline.cpp index cdc7ebfc6a..92158d4fbb 100644 --- a/modules/tracker/tt/src/tools/vpTemplateTrackerBSpline.cpp +++ b/modules/tracker/tt/src/tools/vpTemplateTrackerBSpline.cpp @@ -41,8 +41,7 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS -double vpTemplateTrackerBSpline::getSubPixBspline4(const vpImage &I, - double r, double t) +double vpTemplateTrackerBSpline::getSubPixBspline4(const vpImage &I, double r, double t) { double res = 0; int cr = (int)(r); @@ -56,8 +55,7 @@ double vpTemplateTrackerBSpline::getSubPixBspline4(const vpImage &I, for (int it = -1; it <= 2; it++) { int tt = it + ct; if (tr >= 0 && tr < height && tt >= 0 && tt < width) - res += - Bspline4((double)ir - er) * Bspline4((double)it - et) * I[tr][tt]; + res += Bspline4((double)ir - er) * Bspline4((double)it - et) * I[tr][tt]; } } return res; diff --git a/modules/tracker/tt/src/tools/vpTemplateTrackerTriangle.cpp b/modules/tracker/tt/src/tools/vpTemplateTrackerTriangle.cpp index 4b1c016695..8b9d737bf8 100644 --- a/modules/tracker/tt/src/tools/vpTemplateTrackerTriangle.cpp +++ b/modules/tracker/tt/src/tools/vpTemplateTrackerTriangle.cpp @@ -43,20 +43,17 @@ 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) + : 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) +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; } @@ -64,8 +61,7 @@ vpTemplateTrackerTriangle::vpTemplateTrackerTriangle( /*! Copy operator. */ -vpTemplateTrackerTriangle &vpTemplateTrackerTriangle:: -operator=(const vpTemplateTrackerTriangle &T) +vpTemplateTrackerTriangle &vpTemplateTrackerTriangle::operator=(const vpTemplateTrackerTriangle &T) { minx_temp = T.minx_temp; miny_temp = T.miny_temp; @@ -106,12 +102,10 @@ operator=(const vpTemplateTrackerTriangle &T) The coordinates of the points are defined as a 2 dimension vector with coordinates (x,y). */ -vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpColVector &c1, - const vpColVector &c2, +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) + : 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]); } @@ -121,8 +115,7 @@ vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpColVector &c1, 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.); + Ttemp.init(C1.x / 2., C1.y / 2., C2.x / 2., C2.y / 2., C3.x / 2., C3.y / 2.); return Ttemp; } @@ -131,11 +124,9 @@ vpTemplateTrackerTriangle vpTemplateTrackerTriangle::getPyramidDown() const - 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) +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); } @@ -146,15 +137,12 @@ vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(int x1, int y1, int x2, \param c2 : Second corner. \param c3 : Third corner. */ -vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpImagePoint &c1, - const vpImagePoint &c2, +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) + : 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()); + init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v()); } /*! @@ -162,12 +150,9 @@ vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpImagePoint &c1, - 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) +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); } @@ -180,9 +165,7 @@ vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(double x1, double y1, 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) +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]); } @@ -192,12 +175,9 @@ void vpTemplateTrackerTriangle::init(const vpColVector &c1, \param c2 : Second corner. \param c3 : Third corner. */ -void vpTemplateTrackerTriangle::init(const vpImagePoint &c1, - const vpImagePoint &c2, - const vpImagePoint &c3) +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()); + init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v()); } /*! @@ -206,11 +186,9 @@ void vpTemplateTrackerTriangle::init(const vpImagePoint &c1, - 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) +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); + init((double)x1, (double)y1, (double)x2, (double)y2, (double)x3, (double)y3); } /*! @@ -219,8 +197,7 @@ void vpTemplateTrackerTriangle::init(int x1, int y1, int x2, int y2, int x3, - 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) +void vpTemplateTrackerTriangle::init(double x1, double y1, double x2, double y2, double x3, double y3) { C1.x = x1; C1.y = y1; @@ -300,8 +277,7 @@ bool vpTemplateTrackerTriangle::inTriangle(const int &i, const int &j) const 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); + return (p_ds_uv0 + p_ds_uv1 < 1. + marge_triangle && p_ds_uv0 > -marge_triangle && p_ds_uv1 > -marge_triangle); } /*! @@ -309,8 +285,7 @@ bool vpTemplateTrackerTriangle::inTriangle(const int &i, const int &j) const \param i : Coordinate along the rows. \param j : Coordinate along the columns. */ -bool vpTemplateTrackerTriangle::inTriangle(const double &i, - const double &j) const +bool vpTemplateTrackerTriangle::inTriangle(const double &i, const double &j) const { if (not_good) return false; @@ -323,26 +298,21 @@ bool vpTemplateTrackerTriangle::inTriangle(const double &i, 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); + 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()); -} +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 +void vpTemplateTrackerTriangle::getCorners(vpImagePoint &c1, vpImagePoint &c2, vpImagePoint &c3) const { c1.set_uv(C1.x, C1.y); c2.set_uv(C2.x, C2.y); @@ -367,8 +337,7 @@ void vpTemplateTrackerTriangle::getCorners(std::vector &c) const (x,y). \param c1 : First corner. \param c2 : Second corner. \param c3 : Third corner. */ -void vpTemplateTrackerTriangle::getCorners(vpColVector &c1, vpColVector &c2, - vpColVector &c3) const +void vpTemplateTrackerTriangle::getCorners(vpColVector &c1, vpColVector &c2, vpColVector &c3) const { c1 = getCorner1(); c2 = getCorner2(); @@ -449,15 +418,9 @@ 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; -} +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; -} +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 index a3d81b1f0e..d3abff85fa 100644 --- a/modules/tracker/tt/src/tools/vpTemplateTrackerZone.cpp +++ b/modules/tracker/tt/src/tools/vpTemplateTrackerZone.cpp @@ -51,10 +51,7 @@ /*! Default constructor. */ -vpTemplateTrackerZone::vpTemplateTrackerZone() - : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1) -{ -} +vpTemplateTrackerZone::vpTemplateTrackerZone() : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1) {} /*! Copy constructor. @@ -81,8 +78,7 @@ void vpTemplateTrackerZone::clear() /*! Copy operator. */ -vpTemplateTrackerZone &vpTemplateTrackerZone:: -operator=(const vpTemplateTrackerZone &z) +vpTemplateTrackerZone &vpTemplateTrackerZone::operator=(const vpTemplateTrackerZone &z) { clear(); @@ -112,8 +108,7 @@ operator=(const vpTemplateTrackerZone &z) corner with a right click. */ -void vpTemplateTrackerZone::initClick(const vpImage &I, - bool delaunay) +void vpTemplateTrackerZone::initClick(const vpImage &I, bool delaunay) { Zone.clear(); @@ -136,15 +131,12 @@ void vpTemplateTrackerZone::initClick(const vpImage &I, } else { if (vip.size() % 3 == 2) // draw line between point 2-1 - vpDisplay::displayLine(I, p, vip[vip.size() - 2], vpColor::blue, - 3); + 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); + 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); + vpDisplay::displayLine(I, p, vip[vip.size() - 3], vpColor::blue, 3); } } } @@ -173,9 +165,8 @@ void vpTemplateTrackerZone::initClick(const vpImage &I, - If false, the vector of image points describe triangles. Its size is then a multiple of 3. */ -void vpTemplateTrackerZone::initFromPoints( - const vpImage &I, const std::vector &vip, - bool delaunay) +void vpTemplateTrackerZone::initFromPoints(const vpImage &I, const std::vector &vip, + bool delaunay) { if (delaunay) { if (vip.size() == 3) { @@ -192,8 +183,7 @@ void vpTemplateTrackerZone::initFromPoints( } else { #if VISP_HAVE_OPENCV_VERSION >= 0x020300 // Init Delaunay - cv::Subdiv2D subdiv( - cv::Rect(0, 0, (int)I.getWidth(), (int)I.getHeight())); + 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; @@ -216,8 +206,7 @@ void vpTemplateTrackerZone::initFromPoints( 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)) { + 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]); @@ -226,8 +215,7 @@ void vpTemplateTrackerZone::initFromPoints( initFromPoints(I, vip_delaunay, false); #else - throw vpException(vpException::functionNotImplementedError, - "Delaunay triangulation is not available!"); + throw vpException(vpException::functionNotImplementedError, "Delaunay triangulation is not available!"); #endif } } else { @@ -282,8 +270,7 @@ void vpTemplateTrackerZone::add(const vpTemplateTrackerTriangle &t) bool vpTemplateTrackerZone::inZone(const int &i, const int &j) const { std::vector::const_iterator Iterateurvecteur; - for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); - ++Iterateurvecteur) { + for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) { if (Iterateurvecteur->inTriangle(i, j)) return true; } @@ -299,8 +286,7 @@ bool vpTemplateTrackerZone::inZone(const int &i, const int &j) const bool vpTemplateTrackerZone::inZone(const double &i, const double &j) const { std::vector::const_iterator Iterateurvecteur; - for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); - ++Iterateurvecteur) { + for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) { if (Iterateurvecteur->inTriangle(i, j)) return true; } @@ -314,13 +300,11 @@ bool vpTemplateTrackerZone::inZone(const double &i, const double &j) const 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 +bool vpTemplateTrackerZone::inZone(const int &i, const int &j, unsigned int &id_triangle) const { unsigned int id = 0; std::vector::const_iterator Iterateurvecteur; - for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); - ++Iterateurvecteur) { + for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) { if (Iterateurvecteur->inTriangle(i, j)) { id_triangle = id; return true; @@ -337,13 +321,11 @@ bool vpTemplateTrackerZone::inZone(const int &i, const int &j, 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 +bool vpTemplateTrackerZone::inZone(const double &i, const double &j, unsigned int &id_triangle) const { unsigned int id = 0; std::vector::const_iterator Iterateurvecteur; - for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); - ++Iterateurvecteur) { + for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) { if (Iterateurvecteur->inTriangle(i, j)) { id_triangle = id; return true; @@ -369,12 +351,10 @@ bool vpTemplateTrackerZone::inZone(const double &i, const double &j, } \endcode */ -void vpTemplateTrackerZone::getTriangle(unsigned int i, - vpTemplateTrackerTriangle &T) const +void vpTemplateTrackerZone::getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const { if (i > getNbTriangle() - 1) - throw(vpException(vpException::badValue, - "Cannot get triangle with index %u", i)); + throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i)); T = Zone[i]; } @@ -392,12 +372,10 @@ void vpTemplateTrackerZone::getTriangle(unsigned int i, } \endcode */ -vpTemplateTrackerTriangle -vpTemplateTrackerZone::getTriangle(unsigned int i) const +vpTemplateTrackerTriangle vpTemplateTrackerZone::getTriangle(unsigned int i) const { if (i > getNbTriangle() - 1) - throw(vpException(vpException::badValue, - "Cannot get triangle with index %u", i)); + throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i)); return Zone[i]; } @@ -418,8 +396,7 @@ vpImagePoint vpTemplateTrackerZone::getCenter() const cpt++; } if (!cpt) { - throw(vpException(vpException::divideByZeroError, - "Cannot compute the zone center: size = 0")); + throw(vpException(vpException::divideByZeroError, "Cannot compute the zone center: size = 0")); } xc = xc / cpt; yc = yc / cpt; @@ -466,9 +443,7 @@ vpRect vpTemplateTrackerZone::getBoundingBox() const 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 &I, - const vpColor &col, - const unsigned int thickness) +void vpTemplateTrackerZone::display(const vpImage &I, const vpColor &col, const unsigned int thickness) { std::vector ip; for (unsigned int i = 0; i < Zone.size(); i++) { @@ -485,9 +460,7 @@ void vpTemplateTrackerZone::display(const vpImage &I, 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 &I, - const vpColor &col, - const unsigned int thickness) +void vpTemplateTrackerZone::display(const vpImage &I, const vpColor &col, const unsigned int thickness) { std::vector ip; for (unsigned int i = 0; i < Zone.size(); i++) { @@ -511,9 +484,7 @@ vpTemplateTrackerZone::~vpTemplateTrackerZone() { clear(); } 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 &I, - unsigned int id, - unsigned char gray_level) +void vpTemplateTrackerZone::fillTriangle(vpImage &I, unsigned int id, unsigned char gray_level) { assert(id < getNbTriangle()); vpTemplateTrackerTriangle triangle; @@ -586,8 +557,7 @@ vpImagePoint vpTemplateTrackerZone::getCenter(int borne_x, int borne_y) const } if (!cpt_pt) { - throw(vpException(vpException::divideByZeroError, - "Cannot compute the zone center: size = 0")); + throw(vpException(vpException::divideByZeroError, "Cannot compute the zone center: size = 0")); } x_center = x_center / cpt_pt; diff --git a/modules/tracker/tt/src/vpTemplateTracker.cpp b/modules/tracker/tt/src/vpTemplateTracker.cpp index ee03cd7924..4dd82f7316 100644 --- a/modules/tracker/tt/src/vpTemplateTracker.cpp +++ b/modules/tracker/tt/src/vpTemplateTracker.cpp @@ -42,20 +42,15 @@ #include 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_() + : 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); @@ -82,8 +77,7 @@ void vpTemplateTracker::setGaussianFilterSize(unsigned int new_taill) vpImageFilter::getGaussianDerivativeKernel(fgdG, taillef); } -void vpTemplateTracker::initTracking(const vpImage &I, - vpTemplateTrackerZone &zone) +void vpTemplateTracker::initTracking(const vpImage &I, vpTemplateTrackerZone &zone) { // std::cout<<"\tInitialise reference..."< &I, - const vpColor &col, - const unsigned int thickness) +void vpTemplateTracker::display(const vpImage &I, const vpColor &col, const unsigned int thickness) { if (I.display) { // Only if a display is associated to the image vpTemplateTrackerZone zoneWarped; @@ -387,8 +379,7 @@ int main() } \endcode */ -void vpTemplateTracker::display(const vpImage &I, const vpColor &col, - const unsigned int thickness) +void vpTemplateTracker::display(const vpImage &I, const vpColor &col, const unsigned int thickness) { if (I.display) { // Only if a display is associated to the image vpTemplateTrackerZone zoneWarped; @@ -397,9 +388,8 @@ void vpTemplateTracker::display(const vpImage &I, const vpColor &col, } } -void vpTemplateTracker::computeOptimalBrentGain( - const vpImage &I, vpColVector &tp, double tMI, - vpColVector &direction, double &alpha) +void vpTemplateTracker::computeOptimalBrentGain(const vpImage &I, vpColVector &tp, double tMI, + vpColVector &direction, double &alpha) { vpColVector **ptp; ptp = new vpColVector *[4]; @@ -620,8 +610,7 @@ void vpTemplateTracker::initPyramidal(unsigned int nbLvl, unsigned int l0) pyrInitialised = true; // vpTRACE("fin init_pyramidal"); } -void vpTemplateTracker::initTrackingPyr(const vpImage &I, - vpTemplateTrackerZone &zone) +void vpTemplateTracker::initTrackingPyr(const vpImage &I, vpTemplateTrackerZone &zone) { // vpTRACE("initTrackingPyr"); zoneTrackedPyr[0].copy(zone); @@ -678,8 +667,7 @@ void vpTemplateTracker::initTrackingPyr(const vpImage &I, the sixth corner with a right click. */ -void vpTemplateTracker::initClick(const vpImage &I, - bool delaunay) +void vpTemplateTracker::initClick(const vpImage &I, bool delaunay) { zoneRef_.initClick(I, delaunay); @@ -705,8 +693,7 @@ void vpTemplateTracker::initClick(const vpImage &I, - 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 &I, - const std::vector &v_ip, +void vpTemplateTracker::initFromPoints(const vpImage &I, const std::vector &v_ip, bool delaunay) { zoneRef_.initFromPoints(I, v_ip, delaunay); @@ -728,8 +715,7 @@ void vpTemplateTracker::initFromPoints(const vpImage &I, \param I: Image containing the reference template. \param zone: The zone that describes the reference template. */ -void vpTemplateTracker::initFromZone(const vpImage &I, - const vpTemplateTrackerZone &zone) +void vpTemplateTracker::initFromZone(const vpImage &I, const vpTemplateTrackerZone &zone) { zoneRef_ = zone; diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp index 887a457b92..a83beafd17 100644 --- a/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp +++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp @@ -39,16 +39,14 @@ *****************************************************************************/ #include -void vpTemplateTrackerWarp::warpTriangle(const vpTemplateTrackerTriangle &in, - const vpColVector &p, +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")); + throw(vpException(vpException::dimensionError, "Bad template tracker warp parameters dimension")); } vpColVector S1(2), S2(2), S3(2); vpColVector rS1(2), rS2(2), rS3(2); @@ -61,9 +59,7 @@ void vpTemplateTrackerWarp::warpTriangle(const vpTemplateTrackerTriangle &in, warpX(S3, rS3, p); out.init(rS1, rS2, rS3); } -void vpTemplateTrackerWarp::warpZone(const vpTemplateTrackerZone &in, - const vpColVector &p, - vpTemplateTrackerZone &out) +void vpTemplateTrackerWarp::warpZone(const vpTemplateTrackerZone &in, const vpColVector &p, vpTemplateTrackerZone &out) { vpTemplateTrackerTriangle TR, TT; out.clear(); @@ -74,8 +70,7 @@ void vpTemplateTrackerWarp::warpZone(const vpTemplateTrackerZone &in, } } -double vpTemplateTrackerWarp::getDistanceBetweenZoneAndWarpedZone( - const vpTemplateTrackerZone &Z, const vpColVector &p) +double vpTemplateTrackerWarp::getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z, const vpColVector &p) { unsigned int nb_corners = Z.getNbTriangle() * 3; computeCoeff(p); @@ -90,16 +85,14 @@ double vpTemplateTrackerWarp::getDistanceBetweenZoneAndWarpedZone( 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])); + 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, +void vpTemplateTrackerWarp::warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u, double *v) { computeCoeff(p); @@ -116,9 +109,8 @@ void vpTemplateTrackerWarp::warp(const double *ut0, const double *vt0, } #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) +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); @@ -152,8 +144,7 @@ void vpTemplateTrackerWarp::findWarp(const double *ut0, const double *vt0, dX = X2 - fX1; G += dW_.t() * dX; - erreur += ((u[i] - fX1[0]) * (u[i] - fX1[0]) + - (v[i] - fX1[1]) * (v[i] - fX1[1])); + erreur += ((u[i] - fX1[0]) * (u[i] - fX1[0]) + (v[i] - fX1[1]) * (v[i] - fX1[1])); } vpMatrix::computeHLM(H, lambda, HLM); @@ -164,8 +155,7 @@ void vpTemplateTrackerWarp::findWarp(const double *ut0, const double *vt0, throw(e); } cpt++; - } while ((cpt < 150) && - (sqrt((erreur_prec - erreur) * (erreur_prec - erreur)) > 1e-20)); + } while ((cpt < 150) && (sqrt((erreur_prec - erreur) * (erreur_prec - erreur)) > 1e-20)); // std::cout<<"erreur apres transformation="<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; + 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()")); + throw(vpTrackingException(vpTrackingException::fatalError, + "Division by zero in vpTemplateTrackerWarpHomography::warpX()")); } -void vpTemplateTrackerWarpHomography::dWarp(const vpColVector &X1, - const vpColVector &X2, - const vpColVector & /*ParamM*/, - vpMatrix &dW_) +void vpTemplateTrackerWarpHomography::dWarp(const vpColVector &X1, const vpColVector &X2, + const vpColVector & /*ParamM*/, vpMatrix &dW_) { double j = X1[0]; double i = X1[1]; @@ -156,11 +143,8 @@ void vpTemplateTrackerWarpHomography::dWarp(const vpColVector &X1, } /*compute dw=dw/dx*dw/dp */ -void vpTemplateTrackerWarpHomography::dWarpCompo(const vpColVector & /*X1*/, - const vpColVector &X2, - const vpColVector &ParamM, - const double *dwdp0, - vpMatrix &dW_) +void vpTemplateTrackerWarpHomography::dWarpCompo(const vpColVector & /*X1*/, const vpColVector &X2, + const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW_) { double dwdx0, dwdx1; double dwdy0, dwdy1; @@ -175,34 +159,26 @@ void vpTemplateTrackerWarpHomography::dWarpCompo(const vpColVector & /*X1*/, } } -void vpTemplateTrackerWarpHomography::warpXInv(const vpColVector &vX, - vpColVector &vXres, - const vpColVector &ParamM) +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 + 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); + 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()")); + throw(vpTrackingException(vpTrackingException::fatalError, "Division by zero in " + "vpTemplateTrackerWarpHomography::" + "warpXSpecialInv()")); } -void vpTemplateTrackerWarpHomography::getParamInverse( - const vpColVector &ParamM, vpColVector &ParamMinv) const +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 vpTemplateTrackerWarpHomography::getHomography(const vpColVector &ParamM) const { vpHomography H; for (unsigned int i = 0; i < 3; i++) @@ -217,8 +193,7 @@ vpHomography vpTemplateTrackerWarpHomography::getHomography( return H; } -void vpTemplateTrackerWarpHomography::getParam(const vpHomography &H, - vpColVector &par) const +void vpTemplateTrackerWarpHomography::getParam(const vpHomography &H, vpColVector &par) const { par = 0; for (unsigned int i = 0; i < 3; i++) @@ -231,9 +206,7 @@ void vpTemplateTrackerWarpHomography::getParam(const vpHomography &H, } } -void vpTemplateTrackerWarpHomography::pRondp(const vpColVector &p1, - const vpColVector &p2, - vpColVector &pres) const +void vpTemplateTrackerWarpHomography::pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const { vpHomography H1 = getHomography(p1); vpHomography H2 = getHomography(p2); diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomographySL3.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomographySL3.cpp index 8012110c57..99ceea11d8 100644 --- a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomographySL3.cpp +++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomographySL3.cpp @@ -42,11 +42,8 @@ // 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) +void vpTemplateTrackerWarpHomographySL3::findWarp(const double *ut0, const double *vt0, const double *u, + const double *v, int nb_pt, vpColVector &p) { // std::cout<<"findWarp OVERLOADE"< -vpTemplateTrackerZNCC::vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp) - : vpTemplateTracker(warp), DI(), temp() +vpTemplateTrackerZNCC::vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp) : vpTemplateTracker(warp), DI(), temp() { dW.resize(2, nbParam); G.resize(nbParam); @@ -54,8 +53,7 @@ vpTemplateTrackerZNCC::vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp) DI.resize(2); } -double vpTemplateTrackerZNCC::getCost(const vpImage &I, - const vpColVector &tp) +double vpTemplateTrackerZNCC::getCost(const vpImage &I, const vpColVector &tp) { double IW, Tij; int i, j; @@ -76,8 +74,7 @@ double vpTemplateTrackerZNCC::getCost(const vpImage &I, j2 = X2[0]; i2 = X2[1]; - if ((j2 < I.getWidth() - 1) && (i2 < I.getHeight() - 1) && (i2 > 0) && - (j2 > 0)) { + if ((j2 < I.getWidth() - 1) && (i2 < I.getHeight() - 1) && (i2 > 0) && (j2 > 0)) { Tij = ptTemplate[point].val; if (!blur) IW = I.getValue(i2, j2); @@ -91,8 +88,7 @@ double vpTemplateTrackerZNCC::getCost(const vpImage &I, } ratioPixelIn = (double)Nbpoint / (double)templateSize; if (!Nbpoint) { - throw(vpException(vpException::divideByZeroError, - "Cannot get cost: size = 0")); + throw(vpException(vpException::divideByZeroError, "Cannot get cost: size = 0")); } moyTij = moyTij / Nbpoint; @@ -110,8 +106,7 @@ double vpTemplateTrackerZNCC::getCost(const vpImage &I, j2 = X2[0]; i2 = X2[1]; - if ((j2 < I.getWidth() - 1) && (i2 < I.getHeight() - 1) && (i2 > 0) && - (j2 > 0)) { + if ((j2 < I.getWidth() - 1) && (i2 < I.getHeight() - 1) && (i2 > 0) && (j2 > 0)) { Tij = ptTemplate[point].val; if (!blur) IW = I.getValue(i2, j2); diff --git a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp index a82891403f..d1a1f51f51 100644 --- a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp +++ b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp @@ -40,15 +40,13 @@ #include #include -vpTemplateTrackerZNCCForwardAdditional:: - vpTemplateTrackerZNCCForwardAdditional(vpTemplateTrackerWarp *warp) +vpTemplateTrackerZNCCForwardAdditional::vpTemplateTrackerZNCCForwardAdditional(vpTemplateTrackerWarp *warp) : vpTemplateTrackerZNCC(warp) { useCompositionnal = false; } -void vpTemplateTrackerZNCCForwardAdditional::initHessienDesired( - const vpImage &I) +void vpTemplateTrackerZNCCForwardAdditional::initHessienDesired(const vpImage &I) { if (blur) vpImageFilter::filter(I, BI, fgG, taillef); @@ -85,8 +83,7 @@ void vpTemplateTrackerZNCCForwardAdditional::initHessienDesired( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Tij = ptTemplate[point].val; if (!blur) @@ -115,8 +112,7 @@ void vpTemplateTrackerZNCCForwardAdditional::initHessienDesired( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Tij = ptTemplate[point].val; if (!blur) @@ -140,9 +136,8 @@ void vpTemplateTrackerZNCCForwardAdditional::initHessienDesired( 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[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; @@ -159,8 +154,7 @@ void vpTemplateTrackerZNCCForwardAdditional::initHessienDesired( // std::cout<<"Hdesire = "< &I) +void vpTemplateTrackerZNCCForwardAdditional::trackNoPyr(const vpImage &I) { if (blur) vpImageFilter::filter(I, BI, fgG, taillef); @@ -203,8 +197,7 @@ void vpTemplateTrackerZNCCForwardAdditional::trackNoPyr( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Tij = ptTemplate[point].val; if (!blur) @@ -219,8 +212,7 @@ void vpTemplateTrackerZNCCForwardAdditional::trackNoPyr( } if (!Nbpoint) { - throw(vpException(vpException::divideByZeroError, - "Cannot track the template: no point")); + throw(vpException(vpException::divideByZeroError, "Cannot track the template: no point")); } moyTij = moyTij / Nbpoint; @@ -238,8 +230,7 @@ void vpTemplateTrackerZNCCForwardAdditional::trackNoPyr( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Tij = ptTemplate[point].val; if (!blur) @@ -278,8 +269,7 @@ void vpTemplateTrackerZNCCForwardAdditional::trackNoPyr( double er = (Tij - IW); erreur += (er * er); - denom += - (Tij - moyTij) * (Tij - moyTij) * (IW - moyIW) * (IW - moyIW); + denom += (Tij - moyTij) * (Tij - moyTij) * (IW - moyIW) * (IW - moyIW); delete[] tempt; } } diff --git a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp index 1d8a64e373..98568da8f9 100644 --- a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp +++ b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp @@ -42,16 +42,14 @@ #include #include -vpTemplateTrackerZNCCInverseCompositional:: - vpTemplateTrackerZNCCInverseCompositional(vpTemplateTrackerWarp *warp) - : vpTemplateTrackerZNCC(warp), compoInitialised(false), evolRMS(0), x_pos(), - y_pos(), threshold_RMS(1e-8), moydIrefdp() +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 &I) +void vpTemplateTrackerZNCCInverseCompositional::initCompInverse(const vpImage &I) { // std::cout<<"Initialise precomputed value of Compositionnal // Inverse"< &I) +void vpTemplateTrackerZNCCInverseCompositional::initHessienDesired(const vpImage &I) { initCompInverse(I); @@ -122,8 +119,7 @@ void vpTemplateTrackerZNCCInverseCompositional::initHessienDesired( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Iref = ptTemplate[point].val; if (!blur) @@ -148,9 +144,8 @@ void vpTemplateTrackerZNCCInverseCompositional::initHessienDesired( 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)); + 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; @@ -183,8 +178,7 @@ void vpTemplateTrackerZNCCInverseCompositional::initHessienDesired( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Iref = ptTemplate[point].val; if (!blur) @@ -209,12 +203,10 @@ void vpTemplateTrackerZNCCInverseCompositional::initHessienDesired( 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]); + 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; @@ -240,8 +232,7 @@ void vpTemplateTrackerZNCCInverseCompositional::initHessienDesired( vpColVector dNCC(nbParam); dNCC = (sIcdIref / denom - NCC * dcovarIref / covarIref); vpMatrix d2covarIref(nbParam, nbParam); - d2covarIref = - -(sIcd2Iref - sdIrefdIref + dcovarIref * dcovarIref.t()) / covarIref; + d2covarIref = -(sIcd2Iref - sdIrefdIref + dcovarIref * dcovarIref.t()) / covarIref; #ifdef APPROX_NCC Hdesire = sIcd2Iref / denom; #else @@ -252,8 +243,7 @@ void vpTemplateTrackerZNCCInverseCompositional::initHessienDesired( // std::cout<<"Hdesire = "< &I) +void vpTemplateTrackerZNCCInverseCompositional::trackNoPyr(const vpImage &I) { if (blur) vpImageFilter::filter(I, BI, fgG, taillef); @@ -284,8 +274,7 @@ void vpTemplateTrackerZNCCInverseCompositional::trackNoPyr( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Iref = ptTemplate[point].val; if (!blur) @@ -319,8 +308,7 @@ void vpTemplateTrackerZNCCInverseCompositional::trackNoPyr( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Iref = ptTemplate[point].val; if (!blur) @@ -330,11 +318,9 @@ void vpTemplateTrackerZNCCInverseCompositional::trackNoPyr( double prod = (Ic - moyIc); for (unsigned int it = 0; it < nbParam; it++) - sIcdIref[it] += - prod * (ptTemplate[point].dW[it] - moydIrefdp[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]); + sIrefdIref[it] += (Iref - moyIref) * (ptTemplate[point].dW[it] - moydIrefdp[it]); // double er=(Iref-Ic); // erreur+=(er*er); @@ -373,8 +359,7 @@ void vpTemplateTrackerZNCCInverseCompositional::trackNoPyr( diverge = true; iteration++; - } while ( - (!diverge && (evolRMS > threshold_RMS) && (iteration < iterationMax))); + } while ((!diverge && (evolRMS > threshold_RMS) && (iteration < iterationMax))); // std::cout<<"erreur "<getNbTriangle() * 3; x_pos.resize(nb_corners); @@ -405,8 +389,7 @@ void vpTemplateTrackerZNCCInverseCompositional::initPosEvalRMS( } } -void vpTemplateTrackerZNCCInverseCompositional::computeEvalRMS( - const vpColVector &p_) +void vpTemplateTrackerZNCCInverseCompositional::computeEvalRMS(const vpColVector &p_) { unsigned int nb_corners = zoneTracked->getNbTriangle() * 3; diff --git a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMI.h b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMI.h index f8f53788ff..52de5ed398 100644 --- a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMI.h +++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMI.h @@ -64,17 +64,10 @@ class VISP_EXPORT vpTemplateTrackerMI : public vpTemplateTracker } vpHessienApproximationType; /*! Hessian computation. */ - typedef enum { - USE_HESSIEN_NORMAL, - USE_HESSIEN_DESIRE, - USE_HESSIEN_BEST_COND - } vpHessienType; + 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; + typedef enum { BSPLINE_THIRD_ORDER = 3, BSPLINE_FOURTH_ORDER = 4 } vpBsplineType; protected: vpHessienType hessianComputation; @@ -120,12 +113,8 @@ class VISP_EXPORT vpTemplateTrackerMI : public vpTemplateTracker void computeProba(int &nbpoint); double getCost(const vpImage &I, const vpColVector &tp); double getCost(const vpImage &I) { return getCost(I, p); } - double getNormalizedCost(const vpImage &I, - const vpColVector &tp); - double getNormalizedCost(const vpImage &I) - { - return getNormalizedCost(I, p); - } + double getNormalizedCost(const vpImage &I, const vpColVector &tp); + double getNormalizedCost(const vpImage &I) { return getNormalizedCost(I, p); } virtual void initHessienDesired(const vpImage &I) = 0; virtual void trackNoPyr(const vpImage &I) = 0; void zeroProbabilities(); @@ -154,36 +143,23 @@ class VISP_EXPORT vpTemplateTrackerMI : public vpTemplateTracker // 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) + : 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) { } explicit vpTemplateTrackerMI(vpTemplateTrackerWarp *_warp); ~vpTemplateTrackerMI(); vpMatrix getCovarianceMatrix() const { return covarianceMatrix; } double getMI() const { return MI_postEstimation; } - double getMI(const vpImage &I, int &nc, const int &bspline, - vpColVector &tp); + double getMI(const vpImage &I, int &nc, const int &bspline, vpColVector &tp); double getMI256(const vpImage &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 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); diff --git a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIBSpline.h b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIBSpline.h index 15e2c5fd0b..3c2d1ad5fc 100644 --- a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIBSpline.h +++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIBSpline.h @@ -59,80 +59,52 @@ 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 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, + 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, + 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, + 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, + 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, + 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 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, + static void PutTotPVBsplineNoSecond(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc, 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, + 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 PutTotPVBspline4NoSecond(double *Prt, int &cr, double &er, - int &ct, double &et, int &Nc, + 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, double *dPrt, int &cr, - double &er, int &ct, double &et, - int &Ncb, double *val, + 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, + 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, + 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, + 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, + 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 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); diff --git a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIESM.h b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIESM.h index 668ca31313..cfcbbc599e 100644 --- a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIESM.h +++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIESM.h @@ -96,17 +96,13 @@ class VISP_EXPORT vpTemplateTrackerMIESM : public vpTemplateTrackerMI public: //! Default constructor. vpTemplateTrackerMIESM() - : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), - CompoInitialised(false), HDirect(), HInverse(), HdesireDirect(), - HdesireInverse(), GDirect(), GInverse() + : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), CompoInitialised(false), HDirect(), HInverse(), + HdesireDirect(), HdesireInverse(), GDirect(), GInverse() { } explicit vpTemplateTrackerMIESM(vpTemplateTrackerWarp *_warp); - void setMinimizationMethod(vpMinimizationTypeMIESM method) - { - minimizationMethod = method; - } + 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 index a7f92d5a23..e43046b036 100644 --- a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h +++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h @@ -55,17 +55,11 @@ The algorithm implemented in this class is described in \cite Dame12a and \cite Marchand16a. */ -class VISP_EXPORT vpTemplateTrackerMIForwardAdditional - : public vpTemplateTrackerMI +class VISP_EXPORT vpTemplateTrackerMIForwardAdditional : public vpTemplateTrackerMI { public: /*! Minimization method. */ - typedef enum { - USE_NEWTON, - USE_LMA, - USE_GRADIENT, - USE_QUASINEWTON - } vpMinimizationTypeMIForwardAdditional; + typedef enum { USE_NEWTON, USE_LMA, USE_GRADIENT, USE_QUASINEWTON } vpMinimizationTypeMIForwardAdditional; private: vpMinimizationTypeMIForwardAdditional minimizationMethod; @@ -107,17 +101,13 @@ class VISP_EXPORT vpTemplateTrackerMIForwardAdditional public: //! Default constructor. vpTemplateTrackerMIForwardAdditional() - : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), evolRMS(0), - x_pos(NULL), y_pos(NULL), threshold_RMS(0), p_prec(), G_prec(), - KQuasiNewton() + : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), evolRMS(0), x_pos(NULL), y_pos(NULL), threshold_RMS(0), + p_prec(), G_prec(), KQuasiNewton() { } explicit vpTemplateTrackerMIForwardAdditional(vpTemplateTrackerWarp *_warp); void setThresholdRMS(double threshold) { threshold_RMS = threshold; } - void setMinimizationMethod(vpMinimizationTypeMIForwardAdditional method) - { - minimizationMethod = method; - } + 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 index 07d772a199..6452529561 100644 --- a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h +++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h @@ -55,8 +55,7 @@ The algorithm implemented in this class is described in \cite Dame12a and \cite Marchand16a. */ -class VISP_EXPORT vpTemplateTrackerMIForwardCompositional - : public vpTemplateTrackerMI +class VISP_EXPORT vpTemplateTrackerMIForwardCompositional : public vpTemplateTrackerMI { protected: bool CompoInitialised; @@ -67,8 +66,7 @@ class VISP_EXPORT vpTemplateTrackerMIForwardCompositional void trackNoPyr(const vpImage &I); public: - explicit vpTemplateTrackerMIForwardCompositional( - vpTemplateTrackerWarp *_warp); + explicit 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 index 95a1807787..1ad338615c 100644 --- a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h +++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h @@ -55,17 +55,11 @@ The algorithm implemented in this class is described in \cite Dame12a and \cite Marchand16a. */ -class VISP_EXPORT vpTemplateTrackerMIInverseCompositional - : public vpTemplateTrackerMI +class VISP_EXPORT vpTemplateTrackerMIInverseCompositional : public vpTemplateTrackerMI { public: /*! Minimization method. */ - typedef enum { - USE_NEWTON, - USE_LMA, - USE_GRADIENT, - USE_QUASINEWTON - } vpMinimizationTypeMIInverseCompositional; + typedef enum { USE_NEWTON, USE_LMA, USE_GRADIENT, USE_QUASINEWTON } vpMinimizationTypeMIInverseCompositional; private: vpMinimizationTypeMIInverseCompositional minimizationMethod; @@ -117,22 +111,16 @@ class VISP_EXPORT vpTemplateTrackerMIInverseCompositional 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) + : 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) { } - explicit vpTemplateTrackerMIInverseCompositional( - vpTemplateTrackerWarp *_warp); + explicit 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; - } + 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 index b1b1e3774b..b971640bb8 100644 --- a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMI.cpp +++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMI.cpp @@ -73,19 +73,16 @@ void vpTemplateTrackerMI::setBspline(const vpBsplineType &newbs) /*std::cout< &I, - const vpColVector &tp) +double vpTemplateTrackerMI::getCost(const vpImage &I, const vpColVector &tp) { double MI = 0; int Nbpoint = 0; @@ -178,8 +172,7 @@ double vpTemplateTrackerMI::getCost(const vpImage &I, double 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)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Nbpoint++; double Tij = ptTemplate[point].val; @@ -193,10 +186,9 @@ double vpTemplateTrackerMI::getCost(const vpImage &I, double er = (IW * (Nc - 1)) / 255. - cr; double 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); + // Calcul de l'histogramme joint par interpolation bilinÈaire + // (Bspline ordre 1) + vpTemplateTrackerMIBSpline::PutPVBsplineD(PrtD, cr, er, ct, et, Nc, 1., bspline); } } @@ -247,15 +239,13 @@ double vpTemplateTrackerMI::getCost(const vpImage &I, 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::epsilon()) + if (std::fabs(Prt[r * Ncb_ + t]) > std::numeric_limits::epsilon()) MI += Prt[r * Ncb_ + t] * log(Prt[r * Ncb_ + t]); return -MI; } -double vpTemplateTrackerMI::getNormalizedCost(const vpImage &I, - const vpColVector &tp) +double vpTemplateTrackerMI::getNormalizedCost(const vpImage &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 @@ -284,8 +274,7 @@ double vpTemplateTrackerMI::getNormalizedCost(const vpImage &I, double j2 = X2[0]; double i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Nbpoint++; double Tij = ptTemplate[point].val; if (!blur) @@ -371,8 +360,7 @@ void vpTemplateTrackerMI::computeProba(int &nbpoint) 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++; + d2Prt[((r2 + r) * Ncb_ + (t2 + t)) * nbParam * nbParam + ip * nbParam + it] += *pt++; } } } @@ -427,8 +415,7 @@ void vpTemplateTrackerMI::computeProba(int &nbpoint) if (nbpoint == 0) { // std::cout<<"plus de point dans template suivi"< - std::numeric_limits::epsilon()) + if (std::fabs(Prt[r * Ncb_ + t]) > std::numeric_limits::epsilon()) MI += Prt[r * Ncb_ + t] * log(Prt[r * Ncb_ + t]); } @@ -508,20 +494,13 @@ void vpTemplateTrackerMI::computeHessien(vpMatrix &Hessian) 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; + 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; + 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]); + Hessian[it][jt] += dprtemp[it] * dprtemp[jt] * (1. / Prt[r * Ncb_ + t] - 1. / Pt[t]); /*std::cout<<"Prt[r*Ncb+t]="< &I) +void vpTemplateTrackerMIESM::initHessienDesired(const vpImage &I) { initCompInverse(); std::cout << "Initialise Hessian at Desired position..." << std::endl; @@ -96,8 +94,7 @@ void vpTemplateTrackerMIESM::initHessienDesired( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Nbpoint++; // if(blur) // IW=BI.getValue(i2,j2); @@ -136,8 +133,8 @@ void vpTemplateTrackerMIESM::initHessienDesired( 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) { + 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); @@ -160,8 +157,7 @@ void vpTemplateTrackerMIESM::initHessienDesired( j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight()) && - (j2 < I.getWidth())) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight()) && (j2 < I.getWidth())) { Nbpoint++; // Tij=ptTemplate[point].val; // if(!blur) @@ -261,10 +257,8 @@ void vpTemplateTrackerMIESM::initCompInverse() 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); + ptTemplateSupp[point].Bt[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et); + ptTemplateSupp[point].dBt[it + 1] = vpTemplateTrackerMIBSpline::dBspline4(-it + et); } } CompoInitialised = true; @@ -333,8 +327,7 @@ void vpTemplateTrackerMIESM::trackNoPyr(const vpImage &I) j2 = X2[0]; i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Nbpoint++; // Tij=ptTemplate[point].val; // if(!blur) @@ -363,8 +356,7 @@ void vpTemplateTrackerMIESM::trackNoPyr(const vpImage &I) // std::cout<<"plus de point dans template suivi"< &I) X2[1] = i2; // Warp->computeDenom(X1,p); - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Nbpoint++; // Tij=ptTemplate[point].val; // Tij=Iterateurvecteur->val; @@ -435,7 +426,7 @@ void vpTemplateTrackerMIESM::trackNoPyr(const vpImage &I) // vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(PrtTout, // cr, er, ct, et, Nc, tptemp, nbParam); // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, - //er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian, + // er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian, // hessianComputation==vpHessienType::USE_HESSIEN_DESIRE); delete[] tptemp; diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardAdditional.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardAdditional.cpp index 01d3e09406..440e6e7243 100644 --- a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardAdditional.cpp +++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardAdditional.cpp @@ -44,18 +44,15 @@ #include #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() +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() { useCompositionnal = false; threshold_RMS = 1e-20; } -void vpTemplateTrackerMIForwardAdditional::initHessienDesired( - const vpImage &I) +void vpTemplateTrackerMIForwardAdditional::initHessienDesired(const vpImage &I) { // std::cout<<"Initialise Hessian at Desired position..."<= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Nbpoint++; Tij = ptTemplate[point].val; if (!blur) @@ -115,11 +111,9 @@ void vpTemplateTrackerMIForwardAdditional::initHessienDesired( 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); + 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); + vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline); delete[] tptemp; } @@ -145,8 +139,7 @@ void vpTemplateTrackerMIForwardAdditional::initHessienDesired( } } -void vpTemplateTrackerMIForwardAdditional::trackNoPyr( - const vpImage &I) +void vpTemplateTrackerMIForwardAdditional::trackNoPyr(const vpImage &I) { dW = 0; @@ -196,8 +189,7 @@ void vpTemplateTrackerMIForwardAdditional::trackNoPyr( double j2 = X2[0]; double i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Nbpoint++; double Tij = ptTemplate[point].val; double IW; @@ -228,13 +220,10 @@ void vpTemplateTrackerMIForwardAdditional::trackNoPyr( tptemp[it] = (dW[0][it] * dx + dW[1][it] * dy); //*tptemp++ =dW[0][it]*dIWx+dW[1][it]*dIWy; // std::cout< std::numeric_limits::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)); + 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; @@ -344,16 +330,14 @@ void vpTemplateTrackerMIForwardAdditional::trackNoPyr( iteration++; iterationGlobale++; - } while ((std::fabs(MI - MIprec) > - std::fabs(MI) * std::numeric_limits::epsilon()) && + } while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits::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"<getNbTriangle() * 3; x_pos = new double[nb_corners]; @@ -387,8 +370,7 @@ void vpTemplateTrackerMIForwardAdditional::initPosEvalRMS( } } -void vpTemplateTrackerMIForwardAdditional::computeEvalRMS( - const vpColVector &pw) +void vpTemplateTrackerMIForwardAdditional::computeEvalRMS(const vpColVector &pw) { unsigned int nb_corners = zoneTracked->getNbTriangle() * 3; diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardCompositional.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardCompositional.cpp index 5b3cae0853..452e061d10 100644 --- a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardCompositional.cpp +++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardCompositional.cpp @@ -39,16 +39,14 @@ *****************************************************************************/ #include -vpTemplateTrackerMIForwardCompositional:: - vpTemplateTrackerMIForwardCompositional(vpTemplateTrackerWarp *_warp) +vpTemplateTrackerMIForwardCompositional::vpTemplateTrackerMIForwardCompositional(vpTemplateTrackerWarp *_warp) : vpTemplateTrackerMI(_warp), CompoInitialised(false) { } void vpTemplateTrackerMIForwardCompositional::initCompo() { - std::cout << "Initialise precomputed value of Compositionnal Direct" - << std::endl; + std::cout << "Initialise precomputed value of Compositionnal Direct" << std::endl; ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize]; for (unsigned int point = 0; point < templateSize; point++) { int i = ptTemplate[point].y; @@ -67,16 +65,13 @@ void vpTemplateTrackerMIForwardCompositional::initCompo() 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); + 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 &I) +void vpTemplateTrackerMIForwardCompositional::initHessienDesired(const vpImage &I) { initCompo(); @@ -115,8 +110,7 @@ void vpTemplateTrackerMIForwardCompositional::initHessienDesired( double j2 = X2[0]; double i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Nbpoint++; // Tij=ptTemplate[point].val; if (!blur) @@ -141,8 +135,7 @@ void vpTemplateTrackerMIForwardCompositional::initHessienDesired( // calcul de l'erreur // erreur+=(Tij-IW)*(Tij-IW); - vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, - tptemp, nbParam, bspline); + vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline); delete[] tptemp; } @@ -160,8 +153,7 @@ void vpTemplateTrackerMIForwardCompositional::initHessienDesired( // std::cout<<"\tEnd initialisation..."< &I) +void vpTemplateTrackerMIForwardCompositional::trackNoPyr(const vpImage &I) { if (!CompoInitialised) std::cout << "Compositionnal tracking no initialised\nUse " @@ -214,8 +206,7 @@ void vpTemplateTrackerMIForwardCompositional::trackNoPyr( X2[1] = i2; Warp->computeDenom(X1, p); - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Nbpoint++; // Tij=ptTemplate[point].val; if (!blur) @@ -240,13 +231,10 @@ void vpTemplateTrackerMIForwardCompositional::trackNoPyr( // 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); + 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); + vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline); delete[] tptemp; } @@ -255,8 +243,7 @@ void vpTemplateTrackerMIForwardCompositional::trackNoPyr( // std::cout<<"plus de point dans template suivi"< - std::fabs(MI) * std::numeric_limits::epsilon()) && + } while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits::epsilon()) && (iteration < iterationMax)); // while( (MI!=MIprec) && (iteration< iterationMax) ); nbIteration = iteration; diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIInverseCompositional.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIInverseCompositional.cpp index 96c6fb26f0..a3ba5655d5 100644 --- a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIInverseCompositional.cpp +++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIInverseCompositional.cpp @@ -42,21 +42,16 @@ #include -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) +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 +void vpTemplateTrackerMIInverseCompositional::initTemplateRefBspline(unsigned int ptIndex, double &et) // AY : Optim { - ptTemplateSupp[ptIndex].BtInit = - new double[(1 + nbParam + nbParam * nbParam) * (unsigned int)bspline]; + ptTemplateSupp[ptIndex].BtInit = new double[(1 + nbParam + nbParam * nbParam) * (unsigned int)bspline]; unsigned int index = 0; int endIndex = 1; @@ -82,28 +77,24 @@ void vpTemplateTrackerMIInverseCompositional::initTemplateRefBspline( 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); + 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]; + (*ptd2BspFct)((double)(-it) + et) * ptTemplate[ptIndex].dW[ip] * ptTemplate[ptIndex].dW[ip2]; } } } } -void vpTemplateTrackerMIInverseCompositional::initCompInverse( - const vpImage &I) +void vpTemplateTrackerMIInverseCompositional::initCompInverse(const vpImage &I) { ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize]; 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) { + 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); @@ -141,8 +132,7 @@ void vpTemplateTrackerMIInverseCompositional::initCompInverse( } CompoInitialised = true; } -void vpTemplateTrackerMIInverseCompositional::initHessienDesired( - const vpImage &I) +void vpTemplateTrackerMIInverseCompositional::initHessienDesired(const vpImage &I) { initCompInverse(I); @@ -180,8 +170,7 @@ void vpTemplateTrackerMIInverseCompositional::initHessienDesired( double j2 = X2[0]; double i2 = X2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 1)) { + if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) { Nbpoint++; // Tij=ptTemplate[point].val; @@ -198,17 +187,13 @@ void vpTemplateTrackerMIInverseCompositional::initHessienDesired( // 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) && + 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); + vpTemplateTrackerMIBSpline::PutTotPVBspline3(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam); // { // if(et>0.5){ct++;} // if(er>0.5){cr++;} @@ -218,8 +203,7 @@ void vpTemplateTrackerMIInverseCompositional::initHessienDesired( // er, ptTemplateSupp[point].BtInit, size); // } } else { - vpTemplateTrackerMIBSpline::PutTotPVBspline4( - PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam); + vpTemplateTrackerMIBSpline::PutTotPVBspline4(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam); // { // // ################### AY : Optim @@ -231,8 +215,7 @@ void vpTemplateTrackerMIInverseCompositional::initHessienDesired( // } } } else if (ptTemplateSelect[point] || !useTemplateSelect) - vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt( - PrtTout, cr, er, ct, et, Nc, nbParam, bspline); + vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(PrtTout, cr, er, ct, et, Nc, nbParam, bspline); } } @@ -249,8 +232,7 @@ void vpTemplateTrackerMIInverseCompositional::initHessienDesired( KQuasiNewton = HLMdesireInverse; } -void vpTemplateTrackerMIInverseCompositional::trackNoPyr( - const vpImage &I) +void vpTemplateTrackerMIInverseCompositional::trackNoPyr(const vpImage &I) { if (!CompoInitialised) std::cout << "Compositionnal tracking no initialised\nUse " @@ -310,8 +292,7 @@ void vpTemplateTrackerMIInverseCompositional::trackNoPyr( j2 = x2[0]; i2 = x2[1]; - if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && - (j2 < I.getWidth() - 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)) @@ -329,26 +310,20 @@ void vpTemplateTrackerMIInverseCompositional::trackNoPyr( int cr = (int)tmp; double er = tmp - (double)cr; - if ((ApproxHessian == HESSIAN_NONSECOND || - hessianComputation == - vpTemplateTrackerMI::USE_HESSIEN_DESIRE) && + 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); + 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); + 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); + 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); + vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb, nbParam, bspline); } } } @@ -359,8 +334,7 @@ void vpTemplateTrackerMIInverseCompositional::trackNoPyr( diverge = true; MI = 0; deletePosEvalRMS(); - throw(vpTrackingException(vpTrackingException::notEnoughPointError, - "No points in the template")); + throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template")); } else { // computeProba(Nbpoint); @@ -446,14 +420,11 @@ void vpTemplateTrackerMIInverseCompositional::trackNoPyr( /*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::epsilon()) // DFP + if (std::fabs(s_scal_y) > std::numeric_limits::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)); + 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::fabs(MI) * std::numeric_limits::epsilon()) && + } while ((!diverge) && (std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits::epsilon()) && (iteration < iterationMax) && (evolRMS > threshold_RMS)); // while( (!diverge) && (MI!=MIprec) &&(iteration< // iterationMax)&&(evolRMS>threshold_RMS) ); @@ -536,8 +505,7 @@ void vpTemplateTrackerMIInverseCompositional::trackNoPyr( } } -void vpTemplateTrackerMIInverseCompositional::initPosEvalRMS( - const vpColVector &pw) +void vpTemplateTrackerMIInverseCompositional::initPosEvalRMS(const vpColVector &pw) { unsigned int nb_corners = zoneTracked->getNbTriangle() * 3; x_pos = new double[nb_corners]; @@ -559,8 +527,7 @@ void vpTemplateTrackerMIInverseCompositional::initPosEvalRMS( } } -void vpTemplateTrackerMIInverseCompositional::computeEvalRMS( - const vpColVector &pw) +void vpTemplateTrackerMIInverseCompositional::computeEvalRMS(const vpColVector &pw) { unsigned int nb_corners = zoneTracked->getNbTriangle() * 3; diff --git a/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp b/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp index a5c102dd1e..e6063d1a18 100644 --- a/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp +++ b/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp @@ -41,9 +41,8 @@ #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) +void vpTemplateTrackerMIBSpline::PutPVBsplineD(double *Prt, int cr, double er, int ct, double et, int Nc, double val, + const int °re) { switch (degre) { case 4: @@ -55,9 +54,7 @@ void vpTemplateTrackerMIBSpline::PutPVBsplineD(double *Prt, int cr, double er, } } -void vpTemplateTrackerMIBSpline::PutPVBsplineD3(double *Prt, int cr, - double er, int ct, double et, - int Nc, double val) +void vpTemplateTrackerMIBSpline::PutPVBsplineD3(double *Prt, int cr, double er, int ct, double et, int Nc, double val) { int sr = 0; int st = 0; @@ -77,9 +74,7 @@ void vpTemplateTrackerMIBSpline::PutPVBsplineD3(double *Prt, int cr, } } -void vpTemplateTrackerMIBSpline::PutPVBsplineD4(double *Prt, int cr, - double er, int ct, double et, - int Nc, double val) +void vpTemplateTrackerMIBSpline::PutPVBsplineD4(double *Prt, int cr, double er, int ct, double et, int Nc, double val) { double Bti[4]; @@ -192,9 +187,8 @@ double vpTemplateTrackerMIBSpline::d2Bspline4(double diff) return 0; } -void vpTemplateTrackerMIBSpline::PutTotPVBspline( - double *Prt, int cr, double &er, int ct, double &et, int Nc, double *val, - unsigned int &NbParam, int °ree) +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: @@ -205,9 +199,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline( } } -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) +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: @@ -218,11 +211,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline( } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, int cr, - double &er, int ct, - double &et, int Nc, - double *val, - unsigned int &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; @@ -249,8 +239,7 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, int cr, *ptd2Bti++ = d2Bspline3(it + et); } - double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9 * - (1 + (int)(NbParam + NbParam * NbParam))]; + double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9 * (1 + (int)(NbParam + NbParam * NbParam))]; for (short int ir = -1; ir <= 1; ++ir) { double Br = Bspline3(-ir + er); @@ -268,9 +257,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, int cr, } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline3( - double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct, - double &et, int Ncb, double *val, unsigned int &NbParam) +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; @@ -310,9 +298,7 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3( 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_; + 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]; } @@ -320,9 +306,7 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3( } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double &er, - double *bt, - unsigned int size) +void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double &er, double *bt, unsigned int size) { // double Br; // for(short int ir=-1;ir<=1;++ir) @@ -368,11 +352,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double &er, #undef LSIZE } -void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, int cr, - double er, int ct, - double et, int Nc, - double *val, - unsigned int &NbParam) +void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, int cr, double er, int ct, double et, int Nc, + double *val, unsigned int &NbParam) { double Bti[4]; double dBti[4]; @@ -389,8 +370,7 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, int cr, int NbParam_ = (int)NbParam; - double *pt = - &Prt[(cr * Nc + ct) * 16 * (1 + NbParam_ + NbParam_ * NbParam_)]; + double *pt = &Prt[(cr * Nc + ct) * 16 * (1 + NbParam_ + NbParam_ * NbParam_)]; for (char ir = -1; ir <= 2; ir++) { double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er); ptBti = &Bti[0]; @@ -410,9 +390,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, int cr, } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline4( - double *Prt, double *dPrt, double *d2Prt, int cr, double er, int ct, - double et, int Ncb, double *val, unsigned int &NbParam) +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 Bti[4]; double dBti[4]; @@ -443,8 +422,7 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4( 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_; + 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]; } @@ -455,9 +433,7 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4( } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double &er, - double *bt, - unsigned int size) +void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double &er, double *bt, unsigned int size) { #define LSIZE 12 double *bt0 = &bt[0]; @@ -491,9 +467,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double &er, #undef LSIZE } -void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond( - double *Prt, int &cr, double &er, int &ct, double &et, int &Nc, - double *val, unsigned int &NbParam, int °ree) +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: @@ -504,9 +479,9 @@ void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond( } } -void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond( - double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et, - int &Ncb, double *val, unsigned int &NbParam, int °ree) +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: @@ -517,9 +492,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond( } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond( - double *Prt, int &cr, double &er, int &ct, double &et, int &Nc, - double *val, unsigned int &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; @@ -544,8 +518,7 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond( int NbParam_ = (int)NbParam; - double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9 * - (1 + NbParam_ + NbParam_ * NbParam_)]; + double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9 * (1 + NbParam_ + NbParam_ * NbParam_)]; for (char ir = -1; ir <= 1; ir++) { double Br = Bspline3(-ir + er); ptBti = &Bti[0]; @@ -563,9 +536,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond( } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond( - double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et, - int &Ncb, double *val, unsigned int &NbParam) +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; @@ -610,9 +582,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond( } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond( - double *Prt, int &cr, double &er, int &ct, double &et, int &Nc, - double *val, unsigned int &NbParam) +void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, int &cr, double &er, int &ct, double &et, + int &Nc, double *val, unsigned int &NbParam) { double Bti[4] = {0, 0, 0, 0}; double dBti[4] = {0, 0, 0, 0}; @@ -624,8 +595,7 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond( int NbParam_ = (int)NbParam; - double *pt = - &Prt[(cr * Nc + ct) * 16 * (1 + NbParam_ + NbParam_ * NbParam_)]; + double *pt = &Prt[(cr * Nc + ct) * 16 * (1 + NbParam_ + NbParam_ * NbParam_)]; for (int ir = -1; ir <= 2; ir++) { double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er); for (int it = 0; it <= 3; it++) { @@ -640,9 +610,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond( } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond( - double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et, - int &Ncb, double *val, unsigned int &NbParam) +void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, + double &et, int &Ncb, double *val, unsigned int &NbParam) { double Bti[4] = {0, 0, 0, 0}; double dBti[4] = {0, 0, 0, 0}; @@ -670,9 +639,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond( } } -void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout( - double *PrtTout, int &cr, double &er, int &ct, double &et, int &Nc, - unsigned int &NbParam, int °ree) +void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et, + int &Nc, unsigned int &NbParam, int °ree) { switch (degree) { case 4: @@ -683,9 +651,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout( } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout( - double *PrtTout, int &cr, double &er, int &ct, double &et, int &Nc, - unsigned int &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; @@ -707,8 +674,7 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout( int NbParam_ = (int)NbParam; int NbParam_val = NbParam_ + NbParam_ * NbParam_; - double *pt = &PrtTout[(unsigned int)(((cr + sr) * Nc + (ct + st)) * 9 * - (1 + NbParam_val))]; + double *pt = &PrtTout[(unsigned int)(((cr + sr) * Nc + (ct + st)) * 9 * (1 + NbParam_val))]; for (int ir = -1; ir <= 1; ir++) { double Br = Bspline3(-ir + er); for (int it = 0; it <= 2; it++) { @@ -717,9 +683,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout( } } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout( - double *PrtTout, int &cr, double &er, int &ct, double &et, int &Nc, - unsigned int &NbParam) +void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et, + int &Nc, unsigned int &NbParam) { double Bti[4] = {0, 0, 0, 0}; @@ -729,8 +694,7 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout( int NbParam_ = (int)NbParam; int NbParam_val = NbParam_ + NbParam_ * NbParam_; - double *pt = - &PrtTout[(unsigned int)((cr * Nc + ct) * 16 * (1 + NbParam_val))]; + double *pt = &PrtTout[(unsigned int)((cr * Nc + ct) * 16 * (1 + NbParam_val))]; for (int ir = -1; ir <= 2; ir++) { double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er); for (int it = 0; it <= 3; it++) { @@ -740,11 +704,8 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout( } } -void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(double *Prt, int &cr, - double &er, int &ct, - double &et, int &Ncb, - unsigned int &NbParam, - int °ree) +void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb, + unsigned int &NbParam, int °ree) { switch (degree) { case 4: @@ -755,9 +716,7 @@ void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(double *Prt, int &cr, } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(double *Prt, int &cr, - double &er, int &ct, - double &et, int &Ncb) +void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb) { int sr = 0; @@ -788,9 +747,7 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(double *Prt, int &cr, } } -void vpTemplateTrackerMIBSpline::PutTotPVBspline4Prt(double *Prt, int &cr, - double &er, int &ct, - double &et, int &Ncb) +void vpTemplateTrackerMIBSpline::PutTotPVBspline4Prt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb) { double Bti[4] = {0, 0, 0, 0}; diff --git a/modules/vision/include/visp3/vision/vpBasicKeyPoint.h b/modules/vision/include/visp3/vision/vpBasicKeyPoint.h index 7f2ff02101..c4287fabae 100644 --- a/modules/vision/include/visp3/vision/vpBasicKeyPoint.h +++ b/modules/vision/include/visp3/vision/vpBasicKeyPoint.h @@ -73,30 +73,22 @@ class VISP_EXPORT vpBasicKeyPoint virtual unsigned int buildReference(const vpImage &I) = 0; - virtual unsigned int buildReference(const vpImage &I, - const vpImagePoint &iP, - const unsigned int height, - const unsigned int width) = 0; + virtual unsigned int buildReference(const vpImage &I, const vpImagePoint &iP, + const unsigned int height, const unsigned int width) = 0; - virtual unsigned int buildReference(const vpImage &I, - const vpRect &rectangle) = 0; + virtual unsigned int buildReference(const vpImage &I, const vpRect &rectangle) = 0; virtual unsigned int matchPoint(const vpImage &I) = 0; - virtual unsigned int matchPoint(const vpImage &I, - const vpImagePoint &iP, - const unsigned int height, + virtual unsigned int matchPoint(const vpImage &I, const vpImagePoint &iP, const unsigned int height, const unsigned int width) = 0; - virtual unsigned int matchPoint(const vpImage &I, - const vpRect &rectangle) = 0; + virtual unsigned int matchPoint(const vpImage &I, const vpRect &rectangle) = 0; - virtual void display(const vpImage &Iref, - const vpImage &Icurrent, + virtual void display(const vpImage &Iref, const vpImage &Icurrent, unsigned int size = 3) = 0; - virtual void display(const vpImage &Icurrent, - unsigned int size = 3, + virtual void display(const vpImage &Icurrent, unsigned int size = 3, const vpColor &color = vpColor::green) = 0; /*! @@ -112,10 +104,7 @@ class VISP_EXPORT vpBasicKeyPoint \return The pointer to the complete list of reference points. */ - inline const vpImagePoint *getAllPointsInReferenceImage() - { - return &referenceImagePointsList[0]; - }; + inline const vpImagePoint *getAllPointsInReferenceImage() { return &referenceImagePointsList[0]; }; /*! Get the nth reference point. This point is copied in the vpImagePoint @@ -125,17 +114,14 @@ class VISP_EXPORT vpBasicKeyPoint 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) + 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")); + throw(vpException(vpException::fatalError, "Index of the reference point out of range")); } - referencePoint.set_ij(referenceImagePointsList[index].get_i(), - referenceImagePointsList[index].get_j()); + referencePoint.set_ij(referenceImagePointsList[index].get_i(), referenceImagePointsList[index].get_j()); } /*! @@ -149,20 +135,15 @@ class VISP_EXPORT vpBasicKeyPoint 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) + 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")); + 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()); + referencePoint.set_ij(referenceImagePointsList[matchedReferencePoints[index]].get_i(), + referenceImagePointsList[matchedReferencePoints[index]].get_j()); + currentPoint.set_ij(currentImagePointsList[index].get_i(), currentImagePointsList[index].get_j()); }; /*! @@ -179,24 +160,21 @@ class VISP_EXPORT vpBasicKeyPoint vpImagePoint referencePoint1; vpImagePoint currentPoint; - surf.getMatchedPoints(1, referencePoint1, currentPoint); //Get the first - matched points + 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 + const vpImagePoint* referencePointsList = surf.getAllPointsInReferenceImage(); + // Get the first matched reference point index in the complete reference point list + int index = surf.getIndexInAllReferencePointList(1); + // Get the first matched reference point + referencePoint2 = referencePointsList[index]; \endcode */ - inline unsigned int - getIndexInAllReferencePointList(const unsigned int indexInMatchedPointList) + 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")); + throw(vpException(vpException::fatalError, "Index of the matched reference point out of range")); } return matchedReferencePoints[indexInMatchedPointList]; } @@ -206,20 +184,14 @@ class VISP_EXPORT vpBasicKeyPoint \return the number of reference points. */ - inline unsigned int getReferencePointNumber() const - { - return (unsigned int)referenceImagePointsList.size(); - }; + 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(); - }; + inline unsigned int getMatchedPointNumber() const { return (unsigned int)matchedReferencePoints.size(); }; /*! Return the vector of reference image point. @@ -228,10 +200,7 @@ class VISP_EXPORT vpBasicKeyPoint \return Vector of reference image point. */ - const std::vector &getReferenceImagePointsList() const - { - return referenceImagePointsList; - } + const std::vector &getReferenceImagePointsList() const { return referenceImagePointsList; } /*! Return the vector of current image point. @@ -240,10 +209,7 @@ class VISP_EXPORT vpBasicKeyPoint \return Vector of the current image point. */ - const std::vector &getCurrentImagePointsList() const - { - return currentImagePointsList; - } + const std::vector &getCurrentImagePointsList() const { return currentImagePointsList; } /*! Return the index of the matched associated to the current image point i. @@ -254,10 +220,7 @@ class VISP_EXPORT vpBasicKeyPoint \return The vector of matching index. */ - const std::vector &getMatchedReferencePoints() const - { - return matchedReferencePoints; - } + const std::vector &getMatchedReferencePoints() const { return matchedReferencePoints; } private: virtual void init() = 0; diff --git a/modules/vision/include/visp3/vision/vpCalibration.h b/modules/vision/include/visp3/vision/vpCalibration.h index 1914083883..4ec29a2ca0 100644 --- a/modules/vision/include/visp3/vision/vpCalibration.h +++ b/modules/vision/include/visp3/vision/vpCalibration.h @@ -75,11 +75,11 @@ class VISP_EXPORT vpCalibration 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_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 @@ -90,23 +90,19 @@ class VISP_EXPORT vpCalibration estimation of the distortion. */ } vpCalibrationMethodType; - vpHomogeneousMatrix - cMo; //!< the pose computed for the model without distortion + 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 + 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 + 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; //!< position of the camera in relation to the effector vpHomogeneousMatrix eMc_dist; public: @@ -123,35 +119,26 @@ class VISP_EXPORT vpCalibration // = operator vpCalibration &operator=(const vpCalibration &twinCalibration); - static void calibrationTsai(const std::vector &cMo, - const std::vector &rMe, + static void calibrationTsai(const std::vector &cMo, const std::vector &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 &table_cal, - vpCameraParameters &cam, - double &globalReprojectionError, - bool verbose = false); - - static int - computeCalibrationTsai(const std::vector &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 &I, vpColor color = vpColor::red, - unsigned int thickness = 1, int subsampling_factor = 1); - int displayGrid(vpImage &I, vpColor color = vpColor::yellow, - unsigned int thickness = 1, int subsampling_factor = 1); + int computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo_est, vpCameraParameters &cam_est, + bool verbose = false); + static int computeCalibrationMulti(vpCalibrationMethodType method, std::vector &table_cal, + vpCameraParameters &cam, double &globalReprojectionError, bool verbose = false); + + static int computeCalibrationTsai(const std::vector &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 &I, vpColor color = vpColor::red, unsigned int thickness = 1, + int subsampling_factor = 1); + int displayGrid(vpImage &I, vpColor color = vpColor::yellow, unsigned int thickness = 1, + int subsampling_factor = 1); //! Set the gain for the virtual visual servoing algorithm. static double getLambda() { return gain; } @@ -166,8 +153,7 @@ class VISP_EXPORT vpCalibration int init(); int readData(const char *filename); - static int readGrid(const char *filename, unsigned int &n, - std::list &oX, std::list &oY, + static int readGrid(const char *filename, unsigned int &n, std::list &oX, std::list &oY, std::list &oZ, bool verbose = false); //! set the gain for the virtual visual servoing algorithm @@ -179,24 +165,17 @@ class VISP_EXPORT vpCalibration 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 &table_cal, - vpCameraParameters &cam, - double &globalReprojectionError, + void calibVVS(vpCameraParameters &cam, vpHomogeneousMatrix &cMo, bool verbose = false); + + static void calibVVSMulti(unsigned int nbPose, vpCalibration table_cal[], vpCameraParameters &cam, bool verbose = false); - void calibVVSWithDistortion(vpCameraParameters &cam, - vpHomogeneousMatrix &cMo, bool verbose = false); - static void calibVVSWithDistortionMulti(unsigned int nbPose, - vpCalibration table_cal[], - vpCameraParameters &cam, + static void calibVVSMulti(std::vector &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 &table_cal, vpCameraParameters &cam, - double &globalReprojectionError, bool verbose = false); + static void calibVVSWithDistortionMulti(std::vector &table_cal, vpCameraParameters &cam, + double &globalReprojectionError, bool verbose = false); private: unsigned int npt; //!< number of points used in calibration computation @@ -204,7 +183,7 @@ class VISP_EXPORT vpCalibration LoZ; //!< list of points coordinates (3D in meters) std::list Lip; //!< list of points coordinates (2D in pixels) - double residual; //!< residual in pixel for camera model without distortion + double residual; //!< residual in pixel for camera model without distortion double residual_dist; //!< residual in pixel for perspective projection with //!< distortion model diff --git a/modules/vision/include/visp3/vision/vpCalibrationException.h b/modules/vision/include/visp3/vision/vpCalibrationException.h index 7e8b36847f..180696edaf 100644 --- a/modules/vision/include/visp3/vision/vpCalibrationException.h +++ b/modules/vision/include/visp3/vision/vpCalibrationException.h @@ -96,11 +96,7 @@ class VISP_EXPORT vpCalibrationException : public vpException setMessage(format, args); va_end(args); } - vpCalibrationException(const int id, const std::string &msg) - : vpException(id, msg) - { - ; - } + vpCalibrationException(const int id, const std::string &msg) : vpException(id, msg) { ; } explicit vpCalibrationException(const int id) : vpException(id) { ; } }; diff --git a/modules/vision/include/visp3/vision/vpFernClassifier.h b/modules/vision/include/visp3/vision/vpFernClassifier.h index 99ba86f80e..7981860534 100644 --- a/modules/vision/include/visp3/vision/vpFernClassifier.h +++ b/modules/vision/include/visp3/vision/vpFernClassifier.h @@ -44,9 +44,8 @@ #include -#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 >= 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 #include @@ -106,8 +105,8 @@ since OpenCV-2.0.0 int main() //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); + //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); @@ -138,9 +137,9 @@ since OpenCV-2.0.0 int main() //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++) - { + //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]); } @@ -151,21 +150,20 @@ rectangle vpImagePoint corners[2]; for (int i=0 ; i < 2 ; 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 + // 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++) - { + //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); + // 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 + // Display the matched points fern.display(Ireference, Icurrent); return(0); @@ -245,39 +243,29 @@ class VISP_EXPORT vpFernClassifier : public vpBasicKeyPoint public: vpFernClassifier(); - vpFernClassifier(const std::string &_dataFile, - const std::string &_objectName); + vpFernClassifier(const std::string &_dataFile, const std::string &_objectName); virtual ~vpFernClassifier(); /* build reference */ virtual unsigned int buildReference(const vpImage &I); - virtual unsigned int buildReference(const vpImage &I, - const vpImagePoint &iP, - const unsigned int height, - const unsigned int width); - virtual unsigned int buildReference(const vpImage &I, - const vpRect &rectangle); + virtual unsigned int buildReference(const vpImage &I, const vpImagePoint &iP, + const unsigned int height, const unsigned int width); + virtual unsigned int buildReference(const vpImage &I, const vpRect &rectangle); /* matching */ virtual unsigned int matchPoint(const vpImage &I); - virtual unsigned int matchPoint(const vpImage &I, - const vpImagePoint &iP, - const unsigned int height, + virtual unsigned int matchPoint(const vpImage &I, const vpImagePoint &iP, const unsigned int height, const unsigned int width); - virtual unsigned int matchPoint(const vpImage &I, - const vpRect &rectangle); + virtual unsigned int matchPoint(const vpImage &I, const vpRect &rectangle); /* display */ - virtual void display(const vpImage &Iref, - const vpImage &Icurrent, + virtual void display(const vpImage &Iref, const vpImage &Icurrent, unsigned int size = 3); - virtual void display(const vpImage &Icurrent, - unsigned int size = 3, + virtual void display(const vpImage &Icurrent, unsigned int size = 3, const vpColor &color = vpColor::green); /* io methods */ - void load(const std::string &_dataFile, - const std::string & /*_objectName*/); + void load(const std::string &_dataFile, const std::string & /*_objectName*/); void record(const std::string &_objectName, const std::string &_dataFile); /* accessors */ diff --git a/modules/vision/include/visp3/vision/vpHomography.h b/modules/vision/include/visp3/vision/vpHomography.h index 4496b1cf83..53b05ad1a3 100644 --- a/modules/vision/include/visp3/vision/vpHomography.h +++ b/modules/vision/include/visp3/vision/vpHomography.h @@ -128,10 +128,9 @@ int main() // Compute the coordinates in pixels of the 4 object points in the // camera frame a vpPoint Pa[4]; - std::vector 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 + std::vector 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]); @@ -140,10 +139,9 @@ Project the points from object frame to // Compute the coordinates in pixels of the 4 object points in the // camera frame b vpPoint Pb[4]; - std::vector 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 + std::vector 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 @@ -199,8 +197,7 @@ class VISP_EXPORT vpHomography : public vpArray2D //! 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); + static void initRansac(unsigned int n, double *xb, double *yb, double *xa, double *ya, vpColVector &x); public: vpHomography(); @@ -208,32 +205,26 @@ class VISP_EXPORT vpHomography : public vpArray2D //! 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); + 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); + 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); + 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); + 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(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n); - void computeDisplacement(const vpColVector &nd, vpRotationMatrix &aRb, - vpTranslationVector &atb, vpColVector &n); + void computeDisplacement(const vpColVector &nd, vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n); void eye(); @@ -266,103 +257,64 @@ class VISP_EXPORT vpHomography : public vpArray2D 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 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")); + throw(vpException(vpException::fatalError, "Cannot resize an homography matrix")); }; void save(std::ofstream &f) const; - static void DLT(const std::vector &xb, - const std::vector &yb, - const std::vector &xa, - const std::vector &ya, vpHomography &aHb, - bool normalization = true); - - static void HLM(const std::vector &xb, - const std::vector &yb, - const std::vector &xa, - const std::vector &ya, bool isplanar, - vpHomography &aHb); - - static bool ransac(const std::vector &xb, - const std::vector &yb, - const std::vector &xa, - const std::vector &ya, vpHomography &aHb, - std::vector &inliers, double &residual, - unsigned int nbInliersConsensus, double threshold, - bool normalization = true); - - static vpImagePoint project(const vpCameraParameters &cam, - const vpHomography &bHa, - const vpImagePoint &iPa); + static void DLT(const std::vector &xb, const std::vector &yb, const std::vector &xa, + const std::vector &ya, vpHomography &aHb, bool normalization = true); + + static void HLM(const std::vector &xb, const std::vector &yb, const std::vector &xa, + const std::vector &ya, bool isplanar, vpHomography &aHb); + + static bool ransac(const std::vector &xb, const std::vector &yb, const std::vector &xa, + const std::vector &ya, vpHomography &aHb, std::vector &inliers, double &residual, + unsigned int nbInliersConsensus, double threshold, bool normalization = true); + + 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 &xb, - const std::vector &yb, - const std::vector &xa, - const std::vector &ya, vpHomography &aHb, - std::vector &inlier, double &residual, - double weights_threshold = 0.4, unsigned int niter = 4, - bool normalization = true); + static void robust(const std::vector &xb, const std::vector &yb, const std::vector &xa, + const std::vector &ya, vpHomography &aHb, std::vector &inlier, double &residual, + 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 build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, const vpPlane &bP); - static void computeDisplacement(const vpHomography &aHb, - const vpColVector &nd, - vpRotationMatrix &aRb, + 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 &aHb, vpRotationMatrix &aRb, vpTranslationVector &atb, + vpColVector &n); - static void computeDisplacement(const vpHomography &H, const double x, - const double y, - std::list &vR, - std::list &vT, + static void computeDisplacement(const vpHomography &H, const double x, const double y, + std::list &vR, std::list &vT, std::list &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); + 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, + 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 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 &xb, - const std::vector &yb, - const std::vector &xa, - const std::vector &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 &x, - const std::vector &y, - std::vector &xn, - std::vector &yn, double &xg, + static bool degenerateConfiguration(vpColVector &x, unsigned int *ind, double threshold_area); + static bool degenerateConfiguration(const std::vector &xb, const std::vector &yb, + const std::vector &xa, const std::vector &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 HartleyDenormalization(vpHomography &aHbn, vpHomography &aHb, - double xg1, double yg1, double coef1, + static void HartleyNormalization(const std::vector &x, const std::vector &y, std::vector &xn, + std::vector &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 diff --git a/modules/vision/include/visp3/vision/vpKeyPoint.h b/modules/vision/include/visp3/vision/vpKeyPoint.h index 927618baf2..d269d22a0c 100644 --- a/modules/vision/include/visp3/vision/vpKeyPoint.h +++ b/modules/vision/include/visp3/vision/vpKeyPoint.h @@ -74,8 +74,7 @@ #if defined(VISP_HAVE_OPENCV_XFEATURES2D) // OpenCV >= 3.0.0 #include -#elif defined(VISP_HAVE_OPENCV_NONFREE) && \ - (VISP_HAVE_OPENCV_VERSION >= 0x020400) && \ +#elif defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x020400) && \ (VISP_HAVE_OPENCV_VERSION < 0x030000) #include #endif @@ -141,9 +140,8 @@ int main() vpImage Irefrence; vpImage Icurrent; - vpKeyPoint::vpFilterMatchingType filterType = -vpKeyPoint::ratioDistanceThreshold; vpKeyPoint keypoint("ORB", "ORB", -"BruteForce-Hamming", filterType); + 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 @@ -153,8 +151,8 @@ vpKeyPoint::ratioDistanceThreshold; vpKeyPoint keypoint("ORB", "ORB", // 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); + // 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); @@ -180,16 +178,15 @@ int main() vpImage Ireference; vpImage Icurrent; - vpKeyPoint::vpFilterMatchingType filterType = -vpKeyPoint::ratioDistanceThreshold; vpKeyPoint keypoint("ORB", "ORB", -"BruteForce-Hamming", filterType); + 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++) - { + //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]); } @@ -202,17 +199,16 @@ rectangle vpImagePoint corners[2]; for (int i=0 ; i < 2 ; i++) //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++) - { + //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); + //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); @@ -232,23 +228,23 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint enum vpFilterMatchingType { 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. */ + 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. */ }; /*! Predefined detection method identifier. */ enum vpDetectionMethodType { 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. */ + 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. */ }; /*! Predefined constant for training image format. */ @@ -268,8 +264,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint DETECTOR_BRISK, DETECTOR_GFTT, DETECTOR_SimpleBlob, -#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || \ - (defined(VISP_HAVE_OPENCV_XFEATURES2D)) +#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || (defined(VISP_HAVE_OPENCV_XFEATURES2D)) DETECTOR_STAR, #endif #if defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D) @@ -281,8 +276,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint DETECTOR_AKAZE, DETECTOR_AGAST, #endif -#if (VISP_HAVE_OPENCV_VERSION >= 0x030100) && \ - defined(VISP_HAVE_OPENCV_XFEATURES2D) +#if (VISP_HAVE_OPENCV_VERSION >= 0x030100) && defined(VISP_HAVE_OPENCV_XFEATURES2D) DETECTOR_MSD, #endif #endif @@ -294,8 +288,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint #if (VISP_HAVE_OPENCV_VERSION >= 0x020403) DESCRIPTOR_ORB, DESCRIPTOR_BRISK, -#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || \ - (defined(VISP_HAVE_OPENCV_XFEATURES2D)) +#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || (defined(VISP_HAVE_OPENCV_XFEATURES2D)) DESCRIPTOR_FREAK, DESCRIPTOR_BRIEF, #endif @@ -311,8 +304,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint DESCRIPTOR_LATCH, #endif #endif -#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && \ - defined(VISP_HAVE_OPENCV_XFEATURES2D) +#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && defined(VISP_HAVE_OPENCV_XFEATURES2D) DESCRIPTOR_VGG, DESCRIPTOR_BoostDesc, #endif @@ -320,145 +312,99 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint DESCRIPTOR_TYPE_SIZE }; - vpKeyPoint(const vpFeatureDetectorType &detectorType, - const vpFeatureDescriptorType &descriptorType, - const std::string &matcherName, - const vpFilterMatchingType &filterType = ratioDistanceThreshold); - vpKeyPoint(const std::string &detectorName = "ORB", - const std::string &extractorName = "ORB", + vpKeyPoint(const vpFeatureDetectorType &detectorType, const vpFeatureDescriptorType &descriptorType, + const std::string &matcherName, const vpFilterMatchingType &filterType = ratioDistanceThreshold); + 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 &detectorNames, - const std::vector &extractorNames, + vpKeyPoint(const std::vector &detectorNames, const std::vector &extractorNames, const std::string &matcherName = "BruteForce", const vpFilterMatchingType &filterType = ratioDistanceThreshold); unsigned int buildReference(const vpImage &I); - unsigned int buildReference(const vpImage &I, - const vpImagePoint &iP, - const unsigned int height, + unsigned int buildReference(const vpImage &I, const vpImagePoint &iP, const unsigned int height, const unsigned int width); - unsigned int buildReference(const vpImage &I, - const vpRect &rectangle); + unsigned int buildReference(const vpImage &I, const vpRect &rectangle); - void buildReference(const vpImage &I, - std::vector &trainKeyPoint, - std::vector &points3f, - const bool append = false, const int class_id = -1); - void buildReference(const vpImage &I, - const std::vector &trainKeyPoint, - const cv::Mat &trainDescriptors, - const std::vector &points3f, + void buildReference(const vpImage &I, std::vector &trainKeyPoint, + std::vector &points3f, const bool append = false, const int class_id = -1); + void buildReference(const vpImage &I, const std::vector &trainKeyPoint, + const cv::Mat &trainDescriptors, const std::vector &points3f, const bool append = false, const int class_id = -1); - static void compute3D(const cv::KeyPoint &candidate, - const std::vector &roi, - const vpCameraParameters &cam, + static void compute3D(const cv::KeyPoint &candidate, const std::vector &roi, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point); - static void compute3D(const vpImagePoint &candidate, - const std::vector &roi, - const vpCameraParameters &cam, + static void compute3D(const vpImagePoint &candidate, const std::vector &roi, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, vpPoint &point); - static void compute3DForPointsInPolygons( - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - std::vector &candidates, - const std::vector &polygons, - const std::vector > &roisPt, - std::vector &points, cv::Mat *descriptors = NULL); - - static void compute3DForPointsInPolygons( - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - std::vector &candidates, - const std::vector &polygons, - const std::vector > &roisPt, - std::vector &points, cv::Mat *descriptors = NULL); - - static void compute3DForPointsOnCylinders( - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - std::vector &candidates, - const std::vector &cylinders, - const std::vector > > - &vectorOfCylinderRois, - std::vector &points, cv::Mat *descriptors = NULL); - - static void compute3DForPointsOnCylinders( - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - std::vector &candidates, - const std::vector &cylinders, - const std::vector > > - &vectorOfCylinderRois, - std::vector &points, cv::Mat *descriptors = NULL); - - bool computePose(const std::vector &imagePoints, - const std::vector &objectPoints, - const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, - std::vector &inlierIndex, double &elapsedTime, + static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + std::vector &candidates, + const std::vector &polygons, + const std::vector > &roisPt, + std::vector &points, cv::Mat *descriptors = NULL); + + static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + std::vector &candidates, + const std::vector &polygons, + const std::vector > &roisPt, + std::vector &points, cv::Mat *descriptors = NULL); + + static void + compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + std::vector &candidates, const std::vector &cylinders, + const std::vector > > &vectorOfCylinderRois, + std::vector &points, cv::Mat *descriptors = NULL); + + static void + compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + std::vector &candidates, const std::vector &cylinders, + const std::vector > > &vectorOfCylinderRois, + std::vector &points, cv::Mat *descriptors = NULL); + + bool computePose(const std::vector &imagePoints, const std::vector &objectPoints, + const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector &inlierIndex, + double &elapsedTime, bool (*func)(vpHomogeneousMatrix *) = NULL); + + bool computePose(const std::vector &objectVpPoints, vpHomogeneousMatrix &cMo, std::vector &inliers, + double &elapsedTime, bool (*func)(vpHomogeneousMatrix *) = NULL); + + bool computePose(const std::vector &objectVpPoints, vpHomogeneousMatrix &cMo, std::vector &inliers, + std::vector &inlierIndex, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *) = NULL); - bool computePose(const std::vector &objectVpPoints, - vpHomogeneousMatrix &cMo, std::vector &inliers, - double &elapsedTime, - bool (*func)(vpHomogeneousMatrix *) = NULL); - - bool computePose(const std::vector &objectVpPoints, - vpHomogeneousMatrix &cMo, std::vector &inliers, - std::vector &inlierIndex, - double &elapsedTime, - bool (*func)(vpHomogeneousMatrix *) = NULL); - - void createImageMatching(vpImage &IRef, - vpImage &ICurrent, - vpImage &IMatching); - void createImageMatching(vpImage &ICurrent, + void createImageMatching(vpImage &IRef, vpImage &ICurrent, vpImage &IMatching); + void createImageMatching(vpImage &ICurrent, vpImage &IMatching); - void detect(const vpImage &I, - std::vector &keyPoints, + void detect(const vpImage &I, std::vector &keyPoints, const vpRect &rectangle = vpRect()); - void detect(const cv::Mat &matImg, std::vector &keyPoints, - const cv::Mat &mask = cv::Mat()); - void detect(const vpImage &I, - std::vector &keyPoints, double &elapsedTime, + void detect(const cv::Mat &matImg, std::vector &keyPoints, const cv::Mat &mask = cv::Mat()); + void detect(const vpImage &I, std::vector &keyPoints, double &elapsedTime, const vpRect &rectangle = vpRect()); - void detect(const cv::Mat &matImg, std::vector &keyPoints, - double &elapsedTime, const cv::Mat &mask = cv::Mat()); - - void detectExtractAffine( - const vpImage &I, - std::vector > &listOfKeypoints, - std::vector &listOfDescriptors, - std::vector > *listOfAffineI = NULL); - - void display(const vpImage &IRef, - const vpImage &ICurrent, unsigned int size = 3); - void display(const vpImage &ICurrent, unsigned int size = 3, - const vpColor &color = vpColor::green); - - void displayMatching(const vpImage &IRef, - vpImage &IMatching, - unsigned int crossSize, unsigned int lineThickness = 1, - const vpColor &color = vpColor::green); - void displayMatching(const vpImage &ICurrent, - vpImage &IMatching, - const std::vector &ransacInliers = - std::vector(), - unsigned int crossSize = 3, - unsigned int lineThickness = 1); - - void extract(const vpImage &I, - std::vector &keyPoints, cv::Mat &descriptors, - std::vector *trainPoints = NULL); - void extract(const cv::Mat &matImg, std::vector &keyPoints, - cv::Mat &descriptors, + void detect(const cv::Mat &matImg, std::vector &keyPoints, double &elapsedTime, + const cv::Mat &mask = cv::Mat()); + + void detectExtractAffine(const vpImage &I, std::vector > &listOfKeypoints, + std::vector &listOfDescriptors, + std::vector > *listOfAffineI = NULL); + + void display(const vpImage &IRef, const vpImage &ICurrent, unsigned int size = 3); + void display(const vpImage &ICurrent, unsigned int size = 3, const vpColor &color = vpColor::green); + + void displayMatching(const vpImage &IRef, vpImage &IMatching, unsigned int crossSize, + unsigned int lineThickness = 1, const vpColor &color = vpColor::green); + void displayMatching(const vpImage &ICurrent, vpImage &IMatching, + const std::vector &ransacInliers = std::vector(), + unsigned int crossSize = 3, unsigned int lineThickness = 1); + + void extract(const vpImage &I, std::vector &keyPoints, cv::Mat &descriptors, std::vector *trainPoints = NULL); - void extract(const vpImage &I, - std::vector &keyPoints, cv::Mat &descriptors, - double &elapsedTime, + void extract(const cv::Mat &matImg, std::vector &keyPoints, cv::Mat &descriptors, std::vector *trainPoints = NULL); - void extract(const cv::Mat &matImg, std::vector &keyPoints, - cv::Mat &descriptors, double &elapsedTime, + void extract(const vpImage &I, std::vector &keyPoints, cv::Mat &descriptors, + double &elapsedTime, std::vector *trainPoints = NULL); + void extract(const cv::Mat &matImg, std::vector &keyPoints, cv::Mat &descriptors, double &elapsedTime, std::vector *trainPoints = NULL); /*! @@ -507,19 +453,17 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \return The detector or NULL if the type passed in parameter does not exist. */ - inline cv::Ptr - getDetector(const vpFeatureDetectorType &type) const + inline cv::Ptr getDetector(const vpFeatureDetectorType &type) const { - std::map::const_iterator it_name = - m_mapOfDetectorNames.find(type); + std::map::const_iterator it_name = m_mapOfDetectorNames.find(type); if (it_name == m_mapOfDetectorNames.end()) { std::cerr << "Internal problem with the feature type and the " "corresponding name!" << std::endl; } - std::map >::const_iterator - findDetector = m_detectors.find(it_name->second); + std::map >::const_iterator findDetector = + m_detectors.find(it_name->second); if (findDetector != m_detectors.end()) { return findDetector->second; } @@ -535,11 +479,9 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \return The detector or NULL if the name passed in parameter does not exist. */ - inline cv::Ptr - getDetector(const std::string &name) const + inline cv::Ptr getDetector(const std::string &name) const { - std::map >::const_iterator - findDetector = m_detectors.find(name); + std::map >::const_iterator findDetector = m_detectors.find(name); if (findDetector != m_detectors.end()) { return findDetector->second; } @@ -551,10 +493,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint /*! Get the feature detector name associated to the type. */ - inline std::map getDetectorNames() const - { - return m_mapOfDetectorNames; - } + inline std::map getDetectorNames() const { return m_mapOfDetectorNames; } /*! Get the elapsed time to compute the keypoint extraction. @@ -570,19 +509,17 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \return The descriptor extractor or NULL if the name passed in parameter does not exist. */ - inline cv::Ptr - getExtractor(const vpFeatureDescriptorType &type) const + inline cv::Ptr getExtractor(const vpFeatureDescriptorType &type) const { - std::map::const_iterator it_name = - m_mapOfDescriptorNames.find(type); + std::map::const_iterator it_name = m_mapOfDescriptorNames.find(type); if (it_name == m_mapOfDescriptorNames.end()) { std::cerr << "Internal problem with the feature type and the " "corresponding name!" << std::endl; } - std::map >::const_iterator - findExtractor = m_extractors.find(it_name->second); + std::map >::const_iterator findExtractor = + m_extractors.find(it_name->second); if (findExtractor != m_extractors.end()) { return findExtractor->second; } @@ -598,11 +535,9 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \return The descriptor extractor or NULL if the name passed in parameter does not exist. */ - inline cv::Ptr - getExtractor(const std::string &name) const + inline cv::Ptr getExtractor(const std::string &name) const { - std::map >::const_iterator - findExtractor = m_extractors.find(name); + std::map >::const_iterator findExtractor = m_extractors.find(name); if (findExtractor != m_extractors.end()) { return findExtractor->second; } @@ -614,11 +549,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint /*! Get the feature descriptor extractor name associated to the type. */ - inline std::map - getExtractorNames() const - { - return m_mapOfDescriptorNames; - } + inline std::map getExtractorNames() const { return m_mapOfDescriptorNames; } /*! Get the image format to use when saving training images. @@ -639,10 +570,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \return The matcher pointer. */ - inline cv::Ptr getMatcher() const - { - return m_matcher; - } + inline cv::Ptr getMatcher() const { return m_matcher; } /*! Get the list of matches (correspondences between the indexes of the @@ -650,10 +578,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \return The list of matches. */ - inline std::vector getMatches() const - { - return m_filteredMatches; - } + inline std::vector getMatches() const { return m_filteredMatches; } /*! Get the list of pairs with the correspondence between the matched query @@ -662,16 +587,13 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \return The list of pairs with the correspondence between the matched query and train keypoints. */ - inline std::vector > - getMatchQueryToTrainKeyPoints() const + inline std::vector > getMatchQueryToTrainKeyPoints() const { - std::vector > - matchQueryToTrainKeyPoints(m_filteredMatches.size()); + std::vector > matchQueryToTrainKeyPoints(m_filteredMatches.size()); for (size_t i = 0; i < m_filteredMatches.size(); i++) { matchQueryToTrainKeyPoints.push_back( - std::pair( - m_queryFilteredKeyPoints[(size_t)m_filteredMatches[i].queryIdx], - m_trainKeyPoints[(size_t)m_filteredMatches[i].trainIdx])); + std::pair(m_queryFilteredKeyPoints[(size_t)m_filteredMatches[i].queryIdx], + m_trainKeyPoints[(size_t)m_filteredMatches[i].trainIdx])); } return matchQueryToTrainKeyPoints; } @@ -681,10 +603,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \return The number of train images. */ - inline unsigned int getNbImages() const - { - return static_cast(m_mapOfImages.size()); - } + inline unsigned int getNbImages() const { return static_cast(m_mapOfImages.size()); } void getObjectPoints(std::vector &objectPoints) const; void getObjectPoints(std::vector &objectPoints) const; @@ -712,20 +631,14 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \return The list of Ransac inliers. */ - inline std::vector getRansacInliers() const - { - return m_ransacInliers; - } + inline std::vector getRansacInliers() const { return m_ransacInliers; } /*! Get the list of Ransac outliers. \return The list of Ransac outliers. */ - inline std::vector getRansacOutliers() const - { - return m_ransacOutliers; - } + inline std::vector getRansacOutliers() const { return m_ransacOutliers; } /*! Get the train descriptors matrix. @@ -743,61 +656,42 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint void initMatcher(const std::string &matcherName); - void insertImageMatching(const vpImage &IRef, - const vpImage &ICurrent, - vpImage &IMatching); - void insertImageMatching(const vpImage &ICurrent, + void insertImageMatching(const vpImage &IRef, const vpImage &ICurrent, vpImage &IMatching); + void insertImageMatching(const vpImage &ICurrent, vpImage &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 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 &matches, double &elapsedTime); + void match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors, std::vector &matches, + double &elapsedTime); unsigned int matchPoint(const vpImage &I); - unsigned int matchPoint(const vpImage &I, - const vpImagePoint &iP, const unsigned int height, + unsigned int matchPoint(const vpImage &I, const vpImagePoint &iP, const unsigned int height, const unsigned int width); - unsigned int matchPoint(const vpImage &I, - const vpRect &rectangle); + unsigned int matchPoint(const vpImage &I, const vpRect &rectangle); - bool matchPoint(const vpImage &I, - const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, - bool (*func)(vpHomogeneousMatrix *) = NULL, - const vpRect &rectangle = vpRect()); - bool matchPoint(const vpImage &I, - const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, - double &error, double &elapsedTime, - bool (*func)(vpHomogeneousMatrix *) = NULL, + bool matchPoint(const vpImage &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, + bool (*func)(vpHomogeneousMatrix *) = NULL, const vpRect &rectangle = vpRect()); + bool matchPoint(const vpImage &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, + double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *) = NULL, const vpRect &rectangle = vpRect()); - bool matchPointAndDetect(const vpImage &I, - vpRect &boundingBox, vpImagePoint ¢erOfGravity, - const bool isPlanarObject = true, - std::vector *imPts1 = NULL, - std::vector *imPts2 = NULL, - double *meanDescriptorDistance = NULL, - double *detectionScore = NULL, - const vpRect &rectangle = vpRect()); - - bool matchPointAndDetect(const vpImage &I, - const vpCameraParameters &cam, - vpHomogeneousMatrix &cMo, double &error, - double &elapsedTime, vpRect &boundingBox, - vpImagePoint ¢erOfGravity, - bool (*func)(vpHomogeneousMatrix *) = NULL, - const vpRect &rectangle = vpRect()); + bool matchPointAndDetect(const vpImage &I, vpRect &boundingBox, vpImagePoint ¢erOfGravity, + const bool isPlanarObject = true, std::vector *imPts1 = NULL, + std::vector *imPts2 = NULL, double *meanDescriptorDistance = NULL, + double *detectionScore = NULL, const vpRect &rectangle = vpRect()); + + bool matchPointAndDetect(const vpImage &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, + void saveLearningData(const std::string &filename, const bool binaryMode = false, const bool saveTrainingImages = true); /*! @@ -825,10 +719,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \param method : Detection method (detectionThreshold or detectionScore). */ - inline void setDetectionMethod(const vpDetectionMethodType &method) - { - m_detectionMethod = method; - } + inline void setDetectionMethod(const vpDetectionMethodType &method) { m_detectionMethod = method; } /*! Set and initialize a detector. @@ -856,8 +747,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint initDetector(detectorName); } -#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && \ - VISP_HAVE_OPENCV_VERSION < 0x030000) +#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. @@ -867,8 +757,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \param value : Value to set */ template - inline void setDetectorParameter(const T1 detectorName, - const T2 parameterName, const T3 value) + 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); @@ -917,8 +806,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint initExtractor(extractorName); } -#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && \ - VISP_HAVE_OPENCV_VERSION < 0x030000) +#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. @@ -928,8 +816,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \param value : Value to set */ template - inline void setExtractorParameter(const T1 extractorName, - const T2 parameterName, const T3 value) + 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); @@ -956,10 +843,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \param imageFormat : The image format. */ - inline void setImageFormat(const vpImageFormatType &imageFormat) - { - m_imageFormat = imageFormat; - } + inline void setImageFormat(const vpImageFormatType &imageFormat) { m_imageFormat = imageFormat; } /*! Set and initialize a matcher denominated by his name \p matcherName. @@ -1003,12 +887,10 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint // 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) { + if (filterType == ratioDistanceThreshold || filterType == stdAndRatioDistanceThreshold) { m_useKnn = true; -#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && \ - VISP_HAVE_OPENCV_VERSION < 0x030000) +#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 @@ -1018,8 +900,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint } else { m_useKnn = false; -#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && \ - VISP_HAVE_OPENCV_VERSION < 0x030000) +#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 @@ -1040,8 +921,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint if (factor > 0.0) { m_matchingFactorThreshold = factor; } else { - throw vpException(vpException::badValue, - "The factor must be positive."); + throw vpException(vpException::badValue, "The factor must be positive."); } } @@ -1052,13 +932,10 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint */ inline void setMatchingRatioThreshold(const double ratio) { - if (ratio > 0.0 && - (ratio < 1.0 || - std::fabs(ratio - 1.0) < std::numeric_limits::epsilon())) { + if (ratio > 0.0 && (ratio < 1.0 || std::fabs(ratio - 1.0) < std::numeric_limits::epsilon())) { m_matchingRatioThreshold = ratio; } else { - throw vpException(vpException::badValue, - "The ratio must be in the interval ]0 ; 1]."); + throw vpException(vpException::badValue, "The ratio must be in the interval ]0 ; 1]."); } } @@ -1071,12 +948,10 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint inline void setRansacConsensusPercentage(const double percentage) { if (percentage > 0.0 && - (percentage < 100.0 || std::fabs(percentage - 100.0) < - std::numeric_limits::epsilon())) { + (percentage < 100.0 || std::fabs(percentage - 100.0) < std::numeric_limits::epsilon())) { m_ransacConsensusPercentage = percentage; } else { - throw vpException(vpException::badValue, - "The percentage must be in the interval ]0 ; 100]."); + throw vpException(vpException::badValue, "The percentage must be in the interval ]0 ; 100]."); } } @@ -1091,9 +966,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint if (nbIter > 0) { m_nbRansacIterations = nbIter; } else { - throw vpException( - vpException::badValue, - "The number of iterations must be greater than zero."); + throw vpException(vpException::badValue, "The number of iterations must be greater than zero."); } } @@ -1125,9 +998,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint if (minCount > 0) { m_nbRansacMinInlierCount = minCount; } else { - throw vpException( - vpException::badValue, - "The minimum number of inliers must be greater than zero."); + throw vpException(vpException::badValue, "The minimum number of inliers must be greater than zero."); } } @@ -1142,9 +1013,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint if (threshold > 0.0) { m_ransacThreshold = threshold; } else { - throw vpException( - vpException::badValue, - "The Ransac threshold must be positive as we deal with distance."); + throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance."); } } @@ -1155,13 +1024,9 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \param useAffine : True to use multiple affine transformations, false otherwise */ - inline void setUseAffineDetection(const bool useAffine) - { - m_useAffineDetection = useAffine; - } + inline void setUseAffineDetection(const bool useAffine) { m_useAffineDetection = useAffine; } -#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && \ - VISP_HAVE_OPENCV_VERSION < 0x030000) +#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. @@ -1174,12 +1039,10 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint // 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") { + } 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; + std::cout << " (the filtering method uses a ratio constraint)" << std::endl; } } #endif @@ -1202,10 +1065,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \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; - } + inline void setUseRansacConsensusPercentage(const bool usePercentage) { m_useConsensusPercentage = usePercentage; } /*! Set the flag to choose between the OpenCV or ViSP Ransac pose estimation @@ -1214,10 +1074,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \param ransacVVS : True to use ViSP function, otherwise use OpenCV function */ - inline void setUseRansacVVS(const bool ransacVVS) - { - m_useRansacVVS = ransacVVS; - } + inline void setUseRansacVVS(const bool ransacVVS) { m_useRansacVVS = ransacVVS; } /*! Set the flag to filter matches where multiple query keypoints are matched @@ -1225,10 +1082,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint \param singleMatchFilter : True to use the single match filter. */ - inline void setUseSingleMatchFilter(const bool singleMatchFilter) - { - m_useSingleMatchFilter = singleMatchFilter; - } + inline void setUseSingleMatchFilter(const bool singleMatchFilter) { m_useSingleMatchFilter = singleMatchFilter; } private: //! If true, compute covariance matrix if the user select the pose @@ -1295,8 +1149,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint //! Elapsed time to do the matching. double m_matchingTime; //! List of pairs between the keypoint and the 3D point after the Ransac. - std::vector > - m_matchRansacKeyPointsToPoints; + std::vector > m_matchRansacKeyPointsToPoints; //! Maximum number of iterations for the Ransac method. int m_nbRansacIterations; //! Minimum number of inliers for the Ransac method. @@ -1341,8 +1194,7 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint //! 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 (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. @@ -1364,13 +1216,10 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint //! matched to a single query keypoint bool m_useSingleMatchFilter; - void affineSkew(double tilt, double phi, cv::Mat &img, cv::Mat &mask, - cv::Mat &Ai); + void affineSkew(double tilt, double phi, cv::Mat &img, cv::Mat &mask, cv::Mat &Ai); - double computePoseEstimationError( - const std::vector > - &matchKeyPoints, - const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est); + double computePoseEstimationError(const std::vector > &matchKeyPoints, + const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est); void filterMatches(); @@ -1414,18 +1263,15 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint { public: // maxLevel - The 0-based index of the last pyramid layer - PyramidAdaptedFeatureDetector( - const cv::Ptr &detector, int maxLevel = 2); + PyramidAdaptedFeatureDetector(const cv::Ptr &detector, int maxLevel = 2); // TODO implement read/write virtual bool empty() const; protected: - virtual void detect(cv::InputArray image, - CV_OUT std::vector &keypoints, + virtual void detect(cv::InputArray image, CV_OUT std::vector &keypoints, cv::InputArray mask = cv::noArray()); - virtual void detectImpl(const cv::Mat &image, - std::vector &keypoints, + virtual void detectImpl(const cv::Mat &image, std::vector &keypoints, const cv::Mat &mask = cv::Mat()) const; cv::Ptr detector; @@ -1446,18 +1292,15 @@ class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint /* * Remove keypoints within borderPixels of an image edge. */ - static void runByImageBorder(std::vector &keypoints, - cv::Size imageSize, int borderSize); + static void runByImageBorder(std::vector &keypoints, cv::Size imageSize, int borderSize); /* * Remove keypoints of sizes out of range. */ - static void runByKeypointSize(std::vector &keypoints, - float minSize, float maxSize = FLT_MAX); + static void runByKeypointSize(std::vector &keypoints, float minSize, float maxSize = FLT_MAX); /* * Remove keypoints from some image by mask for pixels of this image. */ - static void runByPixelsMask(std::vector &keypoints, - const cv::Mat &mask); + static void runByPixelsMask(std::vector &keypoints, const cv::Mat &mask); /* * Remove duplicated keypoints. */ diff --git a/modules/vision/include/visp3/vision/vpKeyPointSurf.h b/modules/vision/include/visp3/vision/vpKeyPointSurf.h index 07e67a04ff..f68445aeb5 100644 --- a/modules/vision/include/visp3/vision/vpKeyPointSurf.h +++ b/modules/vision/include/visp3/vision/vpKeyPointSurf.h @@ -104,8 +104,8 @@ int main() { -#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < -0x030000) vpImage Ireference; vpImage Icurrent; +#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000) + vpImage Ireference; vpImage Icurrent; vpKeyPointSurf surf; // First grab the reference image Ireference @@ -115,8 +115,8 @@ int main() // 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); + // 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); @@ -138,15 +138,14 @@ the current image. surf.matchPoint(Icurrent); int main() { -#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < -0x030000) vpImage Ireference; vpImage Icurrent; +#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000) + vpImage Ireference; vpImage 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++) - { + //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]); } @@ -159,17 +158,16 @@ rectangle vpImagePoint corners[2]; for (int i=0 ; i < 2 ; i++) //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++) - { + //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); + //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); @@ -202,28 +200,19 @@ class VISP_EXPORT vpKeyPointSurf : public vpBasicKeyPoint virtual ~vpKeyPointSurf(); unsigned int buildReference(const vpImage &I); - unsigned int buildReference(const vpImage &I, - const vpImagePoint &iP, - const unsigned int height, + unsigned int buildReference(const vpImage &I, const vpImagePoint &iP, const unsigned int height, const unsigned int width); - unsigned int buildReference(const vpImage &I, - const vpRect &rectangle); + unsigned int buildReference(const vpImage &I, const vpRect &rectangle); unsigned int matchPoint(const vpImage &I); - unsigned int matchPoint(const vpImage &I, - const vpImagePoint &iP, const unsigned int height, + unsigned int matchPoint(const vpImage &I, const vpImagePoint &iP, const unsigned int height, const unsigned int width); - unsigned int matchPoint(const vpImage &I, - const vpRect &rectangle); - void display(const vpImage &Iref, - const vpImage &Icurrent, unsigned int size = 3); - void display(const vpImage &Icurrent, unsigned int size = 3, - const vpColor &color = vpColor::green); - std::list *matchPoint(std::list descriptorList, - std::list laplacianList); + unsigned int matchPoint(const vpImage &I, const vpRect &rectangle); + void display(const vpImage &Iref, const vpImage &Icurrent, unsigned int size = 3); + void display(const vpImage &Icurrent, unsigned int size = 3, const vpColor &color = vpColor::green); + std::list *matchPoint(std::list descriptorList, std::list laplacianList); float *getDescriptorReferencePoint(const int index); int getLaplacianReferencePoint(const int index); - void getDescriptorParamReferencePoint(const int index, int &size, - float &dir); + void getDescriptorParamReferencePoint(const int index, int &size, float &dir); /*! Sets the value of the hessian threhold. Note that during the diff --git a/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h b/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h index 1940bc0060..daecfd0b9a 100644 --- a/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h +++ b/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h @@ -49,38 +49,31 @@ #include #include -int VISP_EXPORT qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, - double *qtb, double *x, double *sdiag, double *wa); +int VISP_EXPORT qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag, + double *wa); 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); +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 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 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 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); +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 diff --git a/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h b/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h index 5266ff27be..ace91feb99 100644 --- a/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h +++ b/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h @@ -41,9 +41,8 @@ #include -#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 >= 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 @@ -101,9 +100,9 @@ available since 2.1.0 int main() //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++) - { + //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]); } @@ -116,11 +115,10 @@ rectangle vpImagePoint corners[2]; for (int i=0 ; i < 2 ; i++) //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); + //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){ @@ -129,8 +127,7 @@ planar.matchPoint(Icurrent, corners[0], height, width); planar.getHomography(homography); } else{ - std::cerr << "planar surface not detected in the current image" << -std::endl; + std::cerr << "planar surface not detected in the current image" << std::endl; } return(0); @@ -180,33 +177,28 @@ class VISP_EXPORT vpPlanarObjectDetector public: // constructors and destructors vpPlanarObjectDetector(); - vpPlanarObjectDetector(const std::string &dataFile, - const std::string &objectName); + vpPlanarObjectDetector(const std::string &dataFile, const std::string &objectName); virtual ~vpPlanarObjectDetector(); // main functions // creation of reference unsigned int buildReference(const vpImage &I); - unsigned int buildReference(const vpImage &I, - const vpImagePoint &iP, unsigned int height, + unsigned int buildReference(const vpImage &I, const vpImagePoint &iP, unsigned int height, unsigned int width); - unsigned int buildReference(const vpImage &I, - const vpRect &rectangle); + unsigned int buildReference(const vpImage &I, const vpRect &rectangle); // matching bool matchPoint(const vpImage &I); - bool matchPoint(const vpImage &I, const vpImagePoint &iP, - const unsigned int height, const unsigned int width); + bool matchPoint(const vpImage &I, const vpImagePoint &iP, const unsigned int height, + const unsigned int width); bool matchPoint(const vpImage &I, const vpRect &rectangle); // database management - void recordDetector(const std::string &objectName, - const std::string &dataFile); + void recordDetector(const std::string &objectName, const std::string &dataFile); void load(const std::string &dataFilename, const std::string &objName); // display void display(vpImage &I, bool displayKpts = false); - void display(vpImage &Iref, vpImage &Icurrent, - bool displayKpts = false); + void display(vpImage &Iref, vpImage &Icurrent, bool displayKpts = false); /*! Return the positions of the detected corners. @@ -236,10 +228,7 @@ class VISP_EXPORT vpPlanarObjectDetector \return Number of reference points. */ - inline unsigned int getNbRefPoints() - { - return (unsigned int)currentImagePoints.size(); - } + inline unsigned int getNbRefPoints() { return (unsigned int)currentImagePoints.size(); } /*! Get the i-th reference point. @@ -262,9 +251,7 @@ class VISP_EXPORT vpPlanarObjectDetector 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); + void getMatchedPoints(const unsigned int _index, vpImagePoint &_referencePoint, vpImagePoint &_currentPoint); /*! Set the threshold for the minimal number of point to validate the @@ -272,10 +259,7 @@ class VISP_EXPORT vpPlanarObjectDetector \param _min : the new threshold. */ - void setMinNbPointValidation(const unsigned int _min) - { - this->minNbMatching = _min; - } + void setMinNbPointValidation(const unsigned int _min) { this->minNbMatching = _min; } /*! Get the threshold for the minimal number of point to validate the diff --git a/modules/vision/include/visp3/vision/vpPose.h b/modules/vision/include/visp3/vision/vpPose.h index 82bc312389..7950c9fed6 100644 --- a/modules/vision/include/visp3/vision/vpPose.h +++ b/modules/vision/include/visp3/vision/vpPose.h @@ -81,19 +81,19 @@ class VISP_EXPORT vpPose public: //! Methods that could be used to estimate the pose from points. typedef enum { - LAGRANGE, /*!< Linear Lagrange approach (does't need an initialization) */ - DEMENTHON, /*!< Linear Dementhon aproach (does't need an initialization) - */ - LOWE, /*!< Lowe aproach based on a Levenberg Marquartd non linear - minimization scheme that needs an initialization from Lagrange or - Dementhon aproach */ - RANSAC, /*!< Robust Ransac aproach (does't need an initialization) */ - LAGRANGE_LOWE, /*!< Non linear Lowe aproach initialized by Lagrange - approach */ - DEMENTHON_LOWE, /*!< Non linear Lowe aproach initialized by Dementhon - approach */ - VIRTUAL_VS, /*!< Non linear virtual visual servoing approach that needs an - initialization from Lagrange or Dementhon aproach */ + LAGRANGE, /*!< Linear Lagrange approach (does't need an initialization) */ + DEMENTHON, /*!< Linear Dementhon aproach (does't need an initialization) + */ + LOWE, /*!< Lowe aproach based on a Levenberg Marquartd non linear + minimization scheme that needs an initialization from Lagrange or + Dementhon aproach */ + RANSAC, /*!< Robust Ransac aproach (does't need an initialization) */ + LAGRANGE_LOWE, /*!< Non linear Lowe aproach initialized by Lagrange + approach */ + DEMENTHON_LOWE, /*!< Non linear Lowe aproach initialized by Dementhon + approach */ + VIRTUAL_VS, /*!< Non linear virtual visual servoing approach that needs an + initialization from Lagrange or Dementhon aproach */ DEMENTHON_VIRTUAL_VS, /*!< Non linear virtual visual servoing approach initialized by Dementhon approach */ LAGRANGE_VIRTUAL_VS /*!< Non linear virtual visual servoing approach @@ -101,16 +101,14 @@ class VISP_EXPORT vpPose } vpPoseMethodType; enum FILTERING_RANSAC_FLAGS { - PREFILTER_DUPLICATE_POINTS = - 0x1, /*!< Remove duplicate points before the RANSAC. */ + PREFILTER_DUPLICATE_POINTS = 0x1, /*!< Remove duplicate points before the RANSAC. */ PREFILTER_ALMOST_DUPLICATE_POINTS = 0x2, /*!< Remove almost duplicate points (up to a tolerance) before the RANSAC. */ - PREFILTER_DEGENERATE_POINTS = 0x4, /*!< Remove degenerate points (same 3D - or 2D coordinates) before the - RANSAC. */ - CHECK_DEGENERATE_POINTS = - 0x8 /*!< Check for degenerate points during the RANSAC. */ + PREFILTER_DEGENERATE_POINTS = 0x4, /*!< Remove degenerate points (same 3D + or 2D coordinates) before the + RANSAC. */ + CHECK_DEGENERATE_POINTS = 0x8 /*!< Check for degenerate points during the RANSAC. */ }; unsigned int npt; //!< Number of point used in pose computation @@ -161,28 +159,21 @@ class VISP_EXPORT vpPose class RansacFunctor { public: - RansacFunctor(const vpHomogeneousMatrix &cMo_, - const unsigned int ransacNbInlierConsensus_, - const int ransacMaxTrials_, const double ransacThreshold_, - const unsigned int initial_seed_, - const bool checkDegeneratePoints_, - const std::vector &listOfUniquePoints_, + RansacFunctor(const vpHomogeneousMatrix &cMo_, const unsigned int ransacNbInlierConsensus_, + const int ransacMaxTrials_, const double ransacThreshold_, const unsigned int initial_seed_, + const bool checkDegeneratePoints_, const std::vector &listOfUniquePoints_, bool (*func_)(vpHomogeneousMatrix *)) - : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), - m_cMo(cMo_), m_foundSolution(false), m_func(func_), - m_initial_seed(initial_seed_), - m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), - m_ransacMaxTrials(ransacMaxTrials_), - m_ransacNbInlierConsensus(ransacNbInlierConsensus_), + : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false), + m_func(func_), m_initial_seed(initial_seed_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), + m_ransacMaxTrials(ransacMaxTrials_), m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_) { } RansacFunctor() - : m_best_consensus(), m_checkDegeneratePoints(false), m_cMo(), - m_foundSolution(false), m_func(NULL), m_initial_seed(0), - m_listOfUniquePoints(), m_nbInliers(0), m_ransacMaxTrials(), - m_ransacNbInlierConsensus(), m_ransacThreshold() + : m_best_consensus(), m_checkDegeneratePoints(false), m_cMo(), m_foundSolution(false), m_func(NULL), + m_initial_seed(0), m_listOfUniquePoints(), m_nbInliers(0), m_ransacMaxTrials(), m_ransacNbInlierConsensus(), + m_ransacThreshold() { } @@ -191,10 +182,7 @@ class VISP_EXPORT vpPose // Access the return value. bool getResult() const { return m_foundSolution; } - std::vector getBestConsensus() const - { - return m_best_consensus; - } + std::vector getBestConsensus() const { return m_best_consensus; } vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; } @@ -224,8 +212,7 @@ class VISP_EXPORT vpPose double computeResidualDementhon(const vpHomogeneousMatrix &cMo); // method used in poseDementhonPlan() - int calculArbreDementhon(vpMatrix &b, vpColVector &U, - vpHomogeneousMatrix &cMo); + int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo); public: vpPose(); @@ -234,23 +221,18 @@ class VISP_EXPORT vpPose void addPoints(const std::vector &lP); void clearPoint(); - bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, - bool (*func)(vpHomogeneousMatrix *) = NULL); + bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(vpHomogeneousMatrix *) = NULL); double computeResidual(const vpHomogeneousMatrix &cMo) const; bool coplanar(int &coplanar_plane_type); - void displayModel(vpImage &I, vpCameraParameters &cam, - vpColor col = vpColor::none); - void displayModel(vpImage &I, vpCameraParameters &cam, - vpColor col = vpColor::none); + void displayModel(vpImage &I, vpCameraParameters &cam, vpColor col = vpColor::none); + void displayModel(vpImage &I, vpCameraParameters &cam, vpColor col = vpColor::none); void init(); void poseDementhonPlan(vpHomogeneousMatrix &cMo); void poseDementhonNonPlan(vpHomogeneousMatrix &cMo); - void poseLagrangePlan(vpHomogeneousMatrix &cMo, - const int coplanar_plane_type = 0); + void poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type = 0); void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo); void poseLowe(vpHomogeneousMatrix &cMo); - bool poseRansac(vpHomogeneousMatrix &cMo, - bool (*func)(vpHomogeneousMatrix *) = NULL); + bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(vpHomogeneousMatrix *) = NULL); void poseVirtualVSrobust(vpHomogeneousMatrix &cMo); void poseVirtualVS(vpHomogeneousMatrix &cMo); void printPoint(); @@ -266,30 +248,19 @@ class VISP_EXPORT vpPose } void setVvsIterMax(int nb) { vvsIterMax = nb; } - void setRansacNbInliersToReachConsensus(const unsigned int &nbC) - { - ransacNbInlierConsensus = nbC; - } + void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; } void setRansacThreshold(const double &t) { // Test whether or not t is > 0 if (t > std::numeric_limits::epsilon()) { ransacThreshold = t; } else { - throw vpException( - vpException::badValue, - "The Ransac threshold must be positive as we deal with distance."); + 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 getRansacInlierIndex() const - { - return ransacInlierIndex; - } + unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); } + std::vector getRansacInlierIndex() const { return ransacInlierIndex; } std::vector getRansacInliers() const { return ransacInliers; } /*! @@ -298,10 +269,7 @@ class VISP_EXPORT vpPose \param flag : True if the covariance has to be computed, false otherwise. */ - void setCovarianceComputation(const bool &flag) - { - computeCovariance = flag; - } + void setCovarianceComputation(const bool &flag) { computeCovariance = flag; } /*! Get the covariance matrix computed in the Virtual Visual Servoing @@ -335,10 +303,7 @@ class VISP_EXPORT vpPose \sa setNbParallelRansacThreads */ - inline int getNbParallelRansacThreads() const - { - return nbParallelRansacThreads; - } + inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; } /*! Set the number of threads for the parallel RANSAC implementation. @@ -347,10 +312,7 @@ class VISP_EXPORT vpPose If the number of threads is 0, the number of threads to use is automatically determined with OpenMP. \sa setUseParallelRansac */ - inline void setNbParallelRansacThreads(const int nb) - { - nbParallelRansacThreads = nb; - } + inline void setNbParallelRansacThreads(const int nb) { nbParallelRansacThreads = nb; } /*! \return True if the parallel RANSAC version should be used. @@ -364,10 +326,7 @@ class VISP_EXPORT vpPose \note Need Pthread. */ - inline void setUseParallelRansac(const bool use) - { - useParallelRansac = use; - } + inline void setUseParallelRansac(const bool use) { useParallelRansac = use; } /*! Get the vector of points. @@ -380,26 +339,19 @@ class VISP_EXPORT vpPose return vectorOfPoints; } - static void display(vpImage &I, vpHomogeneousMatrix &cMo, - vpCameraParameters &cam, double size, + static void display(vpImage &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col = vpColor::none); - static void display(vpImage &I, vpHomogeneousMatrix &cMo, - vpCameraParameters &cam, double size, + static void display(vpImage &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 double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, + vpCameraParameters &cam, vpHomogeneousMatrix &cMo); - static int computeRansacIterations(double probability, double epsilon, - const int sampleSize = 4, + static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4, int maxIterations = 2000); static void findMatch(std::vector &p2D, std::vector &p3D, - const unsigned int &numberOfInlierToReachAConsensus, - const double &threshold, unsigned int &ninliers, - std::vector &listInliers, - vpHomogeneousMatrix &cMo, + const unsigned int &numberOfInlierToReachAConsensus, const double &threshold, + unsigned int &ninliers, std::vector &listInliers, vpHomogeneousMatrix &cMo, const int &maxNbTrials = 10000); }; diff --git a/modules/vision/include/visp3/vision/vpPoseException.h b/modules/vision/include/visp3/vision/vpPoseException.h index 1e2e14f611..3a600bcae1 100644 --- a/modules/vision/include/visp3/vision/vpPoseException.h +++ b/modules/vision/include/visp3/vision/vpPoseException.h @@ -92,10 +92,7 @@ class VISP_EXPORT vpPoseException : public vpException setMessage(format, args); va_end(args); } - vpPoseException(const int id, const std::string &msg) : vpException(id, msg) - { - ; - } + vpPoseException(const int id, const std::string &msg) : vpException(id, msg) { ; } explicit vpPoseException(const int id) : vpException(id) { ; } // vpPoseException() : vpException() { ;} }; diff --git a/modules/vision/include/visp3/vision/vpPoseFeatures.h b/modules/vision/include/visp3/vision/vpPoseFeatures.h index e8b6c647c0..318bc66325 100644 --- a/modules/vision/include/visp3/vision/vpPoseFeatures.h +++ b/modules/vision/include/visp3/vision/vpPoseFeatures.h @@ -77,48 +77,36 @@ //## Call a function with a tuple as parameters //################################################# template struct vpDesiredFeatureBuilderWithTuple { - template - static void - buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsF...), - const std::tuple &t, Args &&... args) + template + static void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsF...), const std::tuple &t, + Args &&... args) { - vpDesiredFeatureBuilderWithTuple::buildDesiredFeatureWithTuple( - feature, f, t, std::get(t), args...); + vpDesiredFeatureBuilderWithTuple::buildDesiredFeatureWithTuple(feature, f, t, std::get(t), args...); } }; template <> struct vpDesiredFeatureBuilderWithTuple<0> { - template - static void buildDesiredFeatureWithTuple( - featureType & /* feature */, RetType (*f)(ArgsF...), - const std::tuple & /* t */, Args &&... args) + template + static void buildDesiredFeatureWithTuple(featureType & /* feature */, RetType (*f)(ArgsF...), + const std::tuple & /* t */, Args &&... args) { f(args...); } }; template <> struct vpDesiredFeatureBuilderWithTuple<1> { - template - static void - buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsF...), - const std::tuple &t, Args &&... args) + template + static void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsF...), const std::tuple &t, + Args &&... args) { - vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple( - feature, f, t, feature, args...); + vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple(feature, f, t, feature, args...); } }; -template -void buildDesiredFeatureWithTuple(featureType &feature, - RetType (*f)(ArgsFunc...), - std::tuple const &t) +template +void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsFunc...), std::tuple const &t) { - vpDesiredFeatureBuilderWithTuple::buildDesiredFeatureWithTuple(feature, f, t); + vpDesiredFeatureBuilderWithTuple::buildDesiredFeatureWithTuple(feature, f, t); } //################################################# @@ -127,53 +115,42 @@ void buildDesiredFeatureWithTuple(featureType &feature, //################################################# template struct vpDesiredFeatureBuilderObjectWithTuple { - template - static void buildDesiredFeatureObjectWithTuple( - objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...), - const std::tuple &t, Args &&... args) + template + static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...), + const std::tuple &t, Args &&... args) { - vpDesiredFeatureBuilderObjectWithTuple< - N - 1>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, - std::get(t), - args...); + vpDesiredFeatureBuilderObjectWithTuple::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, + std::get(t), args...); } }; template <> struct vpDesiredFeatureBuilderObjectWithTuple<0> { - template - static void - buildDesiredFeatureObjectWithTuple(objType *obj, featureType & /*feature*/, - RetType (objType::*f)(ArgsF...), - const std::tuple & /* t */, - Args &&... args) + template + static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType & /*feature*/, + RetType (objType::*f)(ArgsF...), const std::tuple & /* t */, + Args &&... args) { (obj->*f)(args...); } }; template <> struct vpDesiredFeatureBuilderObjectWithTuple<1> { - template - static void buildDesiredFeatureObjectWithTuple( - objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...), - const std::tuple &t, Args &&... args) + template + static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...), + const std::tuple &t, Args &&... args) { - vpDesiredFeatureBuilderObjectWithTuple< - 0>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, feature, - args...); + vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, feature, args...); } }; -template -void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, - RetType (objType::*f)(ArgsFunc...), +template +void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsFunc...), std::tuple const &t) { - vpDesiredFeatureBuilderObjectWithTuple::buildDesiredFeatureObjectWithTuple(obj, feature, f, t); + vpDesiredFeatureBuilderObjectWithTuple::buildDesiredFeatureObjectWithTuple(obj, feature, f, t); } //##################################################### @@ -183,56 +160,43 @@ void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, //##################################################### template struct vpCurrentFeatureBuilderWithTuple { - template - static void buildCurrentFeatureWithTuple(featureType &feature, - const vpHomogeneousMatrix &cMo, - RetType (*f)(ArgsF...), - std::tuple &t, - ArgsDecomposed &&... args) + template + static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsF...), + std::tuple &t, ArgsDecomposed &&... args) { auto proj = std::get(t); proj.track(cMo); - vpCurrentFeatureBuilderWithTuple::buildCurrentFeatureWithTuple( - feature, cMo, f, t, proj, args...); + vpCurrentFeatureBuilderWithTuple::buildCurrentFeatureWithTuple(feature, cMo, f, t, proj, args...); } }; template <> struct vpCurrentFeatureBuilderWithTuple<0> { - template - static void buildCurrentFeatureWithTuple( - featureType & /*feature*/, const vpHomogeneousMatrix & /*cMo*/, - RetType (*f)(ArgsF...), std::tuple &, - ArgsDecomposed &&... args) + template + static void buildCurrentFeatureWithTuple(featureType & /*feature*/, const vpHomogeneousMatrix & /*cMo*/, + RetType (*f)(ArgsF...), std::tuple &, + ArgsDecomposed &&... args) { f(args...); } }; template <> struct vpCurrentFeatureBuilderWithTuple<1> { - template - static void buildCurrentFeatureWithTuple(featureType &feature, - const vpHomogeneousMatrix &cMo, - RetType (*f)(ArgsF...), - std::tuple &t, - ArgsDecomposed &&... args) + template + static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsF...), + std::tuple &t, ArgsDecomposed &&... args) { - vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple( - feature, cMo, f, t, feature, args...); + vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple(feature, cMo, f, t, feature, args...); } }; -template -void buildCurrentFeatureWithTuple(featureType &feature, - const vpHomogeneousMatrix &cMo, - RetType (*f)(ArgsFunc...), +template +void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsFunc...), std::tuple &t) { - vpCurrentFeatureBuilderWithTuple::buildCurrentFeatureWithTuple(feature, cMo, f, t); + vpCurrentFeatureBuilderWithTuple::buildCurrentFeatureWithTuple(feature, cMo, f, t); } //##################################################### @@ -243,60 +207,48 @@ void buildCurrentFeatureWithTuple(featureType &feature, //##################################################### template struct vpCurrentFeatureBuilderObjectWithTuple { - template - static void buildCurrentFeatureObjectWithTuple( - objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo, - RetType (objType::*f)(ArgsF...), std::tuple &t, - ArgsDecomposed &&... args) + static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo, + RetType (objType::*f)(ArgsF...), std::tuple &t, + ArgsDecomposed &&... args) { auto proj = std::get(t); proj.track(cMo); - vpCurrentFeatureBuilderObjectWithTuple< - N - 1>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, - proj, args...); + vpCurrentFeatureBuilderObjectWithTuple::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, proj, + args...); } }; template <> struct vpCurrentFeatureBuilderObjectWithTuple<0> { - template - static void buildCurrentFeatureObjectWithTuple( - objType *obj, featureType & /*feature*/, - const vpHomogeneousMatrix & /*cMo*/, RetType (objType::*f)(ArgsF...), - std::tuple &, ArgsDecomposed &&... args) + static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType & /*feature*/, + const vpHomogeneousMatrix & /*cMo*/, RetType (objType::*f)(ArgsF...), + std::tuple &, ArgsDecomposed &&... args) { (obj->*f)(args...); } }; template <> struct vpCurrentFeatureBuilderObjectWithTuple<1> { - template - static void buildCurrentFeatureObjectWithTuple( - objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo, - RetType (objType::*f)(ArgsF...), std::tuple &t, - ArgsDecomposed &&... args) + static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo, + RetType (objType::*f)(ArgsF...), std::tuple &t, + ArgsDecomposed &&... args) { - vpCurrentFeatureBuilderObjectWithTuple< - 0>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, - feature, args...); + vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, feature, + args...); } }; -template -void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, - const vpHomogeneousMatrix &cMo, - RetType (objType::*f)(ArgsFunc...), - std::tuple &t) +template +void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo, + RetType (objType::*f)(ArgsFunc...), std::tuple &t) { - vpCurrentFeatureBuilderObjectWithTuple::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, - t); + vpCurrentFeatureBuilderObjectWithTuple::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, + t); } //################################################# @@ -349,10 +301,7 @@ class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature virtual ~vpPoseSpecificFeatureTemplate() { delete tuple; }; - virtual void createDesired() - { - buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple); - } + virtual void createDesired() { buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple); } virtual vpColVector error() { @@ -361,10 +310,7 @@ class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature return currentFeature.error(desiredFeature); } - virtual vpMatrix currentInteraction() - { - return currentFeature.interaction(); - } + virtual vpMatrix currentInteraction() { return currentFeature.interaction(); } virtual void createCurrent(const vpHomogeneousMatrix &cMo) { @@ -383,8 +329,7 @@ class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature \brief Template class that allows to estimate a pose from all kind of specific features if the compiler support C++ 11. */ -template +template class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature { private: @@ -395,9 +340,7 @@ class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature ObjectType *obj; public: - vpPoseSpecificFeatureTemplateObject(ObjectType *o, - RetType (ObjectType::*f_ptr)(Args...), - Args &&... args) + vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType (ObjectType::*f_ptr)(Args...), Args &&... args) { func_ptr = f_ptr; // std::move(f_ptr); tuple = new std::tuple(args...); @@ -406,22 +349,15 @@ class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature virtual ~vpPoseSpecificFeatureTemplateObject() { delete tuple; }; - virtual void createDesired() - { - buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple); - } + virtual void createDesired() { buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple); } virtual vpColVector error() { return currentFeature.error(desiredFeature); } - virtual vpMatrix currentInteraction() - { - return currentFeature.interaction(); - } + virtual vpMatrix currentInteraction() { return currentFeature.interaction(); } virtual void createCurrent(const vpHomogeneousMatrix &cMo) { - buildCurrentFeatureObjectWithTuple(obj, currentFeature, cMo, func_ptr, - *tuple); + buildCurrentFeatureObjectWithTuple(obj, currentFeature, cMo, func_ptr, *tuple); } }; #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -456,9 +392,7 @@ class VISP_EXPORT vpPoseFeatures vpDuo() : desiredFeature(NULL), firstParam() {} }; - template - struct vpTrio { + template struct vpTrio { FeatureType *desiredFeature; FirstParamType firstParam; SecondParamType secondParam; @@ -482,20 +416,16 @@ class VISP_EXPORT vpPoseFeatures // vpFeaturePoint3D std::vector > featurePoint3D_Point_list; // vpFeatureVanishingPoint - std::vector > - featureVanishingPoint_Point_list; - std::vector > - featureVanishingPoint_DuoLine_list; + std::vector > featureVanishingPoint_Point_list; + std::vector > featureVanishingPoint_DuoLine_list; // vpFeatureEllipse std::vector > featureEllipse_Sphere_list; std::vector > featureEllipse_Circle_list; // vpFeatureLine std::vector > featureLine_Line_list; - std::vector > - featureLine_DuoLineInt_List; + std::vector > featureLine_DuoLineInt_List; // vpFeatureSegment - std::vector > - featureSegment_DuoPoints_list; + std::vector > featureSegment_DuoPoints_list; #ifdef VISP_HAVE_CPP11_COMPATIBILITY // Specific features @@ -526,18 +456,14 @@ class VISP_EXPORT vpPoseFeatures template void addSpecificFeature(RetType (*fct_ptr)(ArgsFunc...), Args &&... args); - template - void addSpecificFeature(ObjType *obj, - RetType (ObjType::*fct_ptr)(ArgsFunc...), - Args &&... args); + template + 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); + void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS); /*! Get the covariance matrix of the pose parameters computed by virtual @@ -576,10 +502,7 @@ class VISP_EXPORT vpPoseFeatures \param flag : True if the covariance has to be computed, false otherwise. */ - void setCovarianceComputation(const bool &flag) - { - computeCovariance = flag; - } + void setCovarianceComputation(const bool &flag) { computeCovariance = flag; } /*! Set the gain used in the virtual visual servoing scheme : \f${\bf v} = @@ -604,8 +527,7 @@ class VISP_EXPORT vpPoseFeatures void setVerbose(const bool &mode) { verbose = mode; } private: - void error_and_interaction(vpHomogeneousMatrix &cMo, vpColVector &err, - vpMatrix &L); + void error_and_interaction(vpHomogeneousMatrix &cMo, vpColVector &err, vpMatrix &L); void computePoseVVS(vpHomogeneousMatrix &cMo); void computePoseRobustVVS(vpHomogeneousMatrix &cMo); @@ -668,16 +590,12 @@ int main() \endcode */ template -void vpPoseFeatures::addSpecificFeature(RetType (*fct_ptr)(ArgsFunc...), - Args &&... args) +void vpPoseFeatures::addSpecificFeature(RetType (*fct_ptr)(ArgsFunc...), Args &&... args) { - typedef typename std::tuple_element<0, std::tuple >::type - featureTypeReference; - typedef - typename std::remove_reference::type featureType; + typedef typename std::tuple_element<0, std::tuple >::type featureTypeReference; + typedef typename std::remove_reference::type featureType; featureSpecific_list.push_back( - new vpPoseSpecificFeatureTemplate( - fct_ptr, std::forward(args)...)); + new vpPoseSpecificFeatureTemplate(fct_ptr, std::forward(args)...)); featureSpecific_list.back()->createDesired(); @@ -707,16 +625,16 @@ class vp_createClass{ public: vp_createClass(){} - int vp_createPoint(vpFeaturePoint &fp,const vpPoint &p){ + 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_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){ + void vp_createLine(vpFeatureLine &fp,const vpLine &l) { vpFeatureBuilder::create(fp,l); } }; @@ -734,12 +652,12 @@ int main() 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; + 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]); @@ -753,19 +671,13 @@ vpPoint&) = &vp_createClass::vp_createTwoPoint; void } \endcode */ -template -void vpPoseFeatures::addSpecificFeature( - ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc...), Args &&... args) +template +void vpPoseFeatures::addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc...), Args &&... args) { - typedef typename std::tuple_element<0, std::tuple >::type - featureTypeReference; - typedef - typename std::remove_reference::type featureType; - featureSpecific_list.push_back( - new vpPoseSpecificFeatureTemplateObject( - obj, fct_ptr, std::forward(args)...)); + typedef typename std::tuple_element<0, std::tuple >::type featureTypeReference; + typedef typename std::remove_reference::type featureType; + featureSpecific_list.push_back(new vpPoseSpecificFeatureTemplateObject( + obj, fct_ptr, std::forward(args)...)); featureSpecific_list.back()->createDesired(); diff --git a/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h b/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h index 255b036923..bb3b768e6d 100644 --- a/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h +++ b/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h @@ -73,12 +73,12 @@ 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. */ + 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. */ @@ -89,39 +89,39 @@ class VISP_EXPORT vpXmlConfigParserKeyPoint : public vpXmlParser 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. */ + 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. */ + 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: @@ -187,30 +187,21 @@ class VISP_EXPORT vpXmlConfigParserKeyPoint : public vpXmlParser \return The factor value for the filtering method: constantFactorDistanceThreshold. */ - inline double getMatchingFactorThreshold() const - { - return m_matchingFactorThreshold; - } + inline double getMatchingFactorThreshold() const { return m_matchingFactorThreshold; } /*! Get the filtering method. \return The filtering method. */ - inline vpMatchingMethodEnum getMatchingMethod() const - { - return m_matchingMethod; - } + 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; - } + inline double getMatchingRatioThreshold() const { return m_matchingRatioThreshold; } /*! Get the maximum number of iterations for the Ransac method. @@ -224,20 +215,14 @@ class VISP_EXPORT vpXmlConfigParserKeyPoint : public vpXmlParser \return The minimum number of inliers for the Ransac method. */ - inline int getNbRansacMinInlierCount() const - { - return m_nbRansacMinInlierCount; - } + 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; - } + inline double getRansacConsensusPercentage() const { return m_ransacConsensusPercentage; } /*! Get the maximum reprojection error for a candidate inlier for the Ransac @@ -245,10 +230,7 @@ class VISP_EXPORT vpXmlConfigParserKeyPoint : public vpXmlParser \return The maximum reprojection error for the Ransac method. */ - inline double getRansacReprojectionError() const - { - return m_ransacReprojectionError; - } + inline double getRansacReprojectionError() const { return m_ransacReprojectionError; } /*! Get the maximum error for a candidate inlier for the Ransac method. @@ -263,10 +245,7 @@ class VISP_EXPORT vpXmlConfigParserKeyPoint : public vpXmlParser \return True to use a percentage value for inliers, false otherwise. */ - inline bool getUseRansacConsensusPercentage() const - { - return m_useRansacConsensusPercentage; - } + inline bool getUseRansacConsensusPercentage() const { return m_useRansacConsensusPercentage; } /*! Get the flag state to choose between OpenCV Ransac pose estimation or ViSP diff --git a/modules/vision/src/calibration/vpCalibration.cpp b/modules/vision/src/calibration/vpCalibration.cpp index 2962d1c5c3..bf2c5178da 100644 --- a/modules/vision/src/calibration/vpCalibration.cpp +++ b/modules/vision/src/calibration/vpCalibration.cpp @@ -72,8 +72,8 @@ int vpCalibration::init() 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.) + : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), npt(0), LoX(), LoY(), LoZ(), Lip(), residual(1000.), + residual_dist(1000.) { init(); } @@ -81,8 +81,8 @@ vpCalibration::vpCalibration() 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.) + : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), npt(0), LoX(), LoY(), LoZ(), Lip(), residual(1000.), + residual_dist(1000.) { (*this) = c; } @@ -159,8 +159,7 @@ int vpCalibration::addPoint(double X, double Y, double Z, vpImagePoint &ip) \param camera : camera intrinsic parameters used for computation. \param cMo_est : computed pose */ -void vpCalibration::computePose(const vpCameraParameters &camera, - vpHomogeneousMatrix &cMo_est) +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) // ) @@ -219,8 +218,7 @@ void vpCalibration::computePose(const vpCameraParameters &camera, \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 vpCalibration::computeStdDeviation(const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &camera) { double residual_ = 0; @@ -240,12 +238,9 @@ double vpCalibration::computeStdDeviation(const vpHomogeneousMatrix &cMo_est, 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 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; @@ -274,9 +269,7 @@ double vpCalibration::computeStdDeviation(const vpHomogeneousMatrix &cMo_est, \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 vpCalibration::computeStdDeviation_dist(const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &camera) { double residual_ = 0; @@ -301,12 +294,9 @@ vpCalibration::computeStdDeviation_dist(const vpHomogeneousMatrix &cMo_est, 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 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; @@ -322,8 +312,7 @@ vpCalibration::computeStdDeviation_dist(const vpHomogeneousMatrix &cMo_est, residual_ += (vpMath::sqr(xp - u) + vpMath::sqr(yp - v)); - double r2du = - (vpMath::sqr((u - u0) * inv_px) + vpMath::sqr((v - v0) * inv_py)); + 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; @@ -347,8 +336,7 @@ vpCalibration::computeStdDeviation_dist(const vpHomogeneousMatrix &cMo_est, distortion. \param deviation_dist : the standard deviation computed for the model with distortion. */ -void vpCalibration::computeStdDeviation(double &deviation, - double &deviation_dist) +void vpCalibration::computeStdDeviation(double &deviation, double &deviation_dist) { deviation = computeStdDeviation(cMo, cam); deviation_dist = computeStdDeviation_dist(cMo_dist, cam_dist); @@ -365,10 +353,8 @@ void vpCalibration::computeStdDeviation(double &deviation, \return 0 if the calibration computation succeed. */ -int vpCalibration::computeCalibration(vpCalibrationMethodType method, - vpHomogeneousMatrix &cMo_est, - vpCameraParameters &cam_est, - bool verbose) +int vpCalibration::computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo_est, + vpCameraParameters &cam_est, bool verbose) { try { computePose(cam_est, cMo_est); @@ -456,10 +442,8 @@ int vpCalibration::computeCalibration(vpCalibrationMethodType method, \return 0 if the computation was managed succeed. */ -int vpCalibration::computeCalibrationMulti( - vpCalibrationMethodType method, std::vector &table_cal, - vpCameraParameters &cam_est, double &globalReprojectionError, - bool verbose) +int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method, std::vector &table_cal, + vpCameraParameters &cam_est, double &globalReprojectionError, bool verbose) { try { unsigned int nbPose = (unsigned int)table_cal.size(); @@ -470,8 +454,7 @@ int vpCalibration::computeCalibrationMulti( switch (method) { case CALIB_LAGRANGE: { if (nbPose > 1) { - std::cout << "this calibration method is not available in" - << std::endl + std::cout << "this calibration method is not available in" << std::endl << "vpCalibration::computeCalibrationMulti()" << std::endl; return -1; } else { @@ -485,8 +468,7 @@ int vpCalibration::computeCalibrationMulti( 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 + std::cout << "this calibration method is not available in" << std::endl << "vpCalibration::computeCalibrationMulti()" << std::endl << "with several images." << std::endl; return -1; @@ -523,8 +505,7 @@ int vpCalibration::computeCalibrationMulti( if (verbose) std::cout << "Compute camera parameters with distortion" << std::endl; - calibVVSWithDistortionMulti(table_cal, cam_est, globalReprojectionError, - verbose); + calibVVSWithDistortionMulti(table_cal, cam_est, globalReprojectionError, verbose); } break; } // Print camera parameters @@ -559,9 +540,8 @@ int vpCalibration::computeCalibrationMulti( 0 if the computation managed, -1 if less than three poses are provides as input. */ -int vpCalibration::computeCalibrationTsai( - const std::vector &table_cal, vpHomogeneousMatrix &eMc, - vpHomogeneousMatrix &eMc_dist) +int vpCalibration::computeCalibrationTsai(const std::vector &table_cal, vpHomogeneousMatrix &eMc, + vpHomogeneousMatrix &eMc_dist) { try { unsigned int nbPose = (unsigned int)table_cal.size(); @@ -580,8 +560,7 @@ int vpCalibration::computeCalibrationTsai( return 0; } else { - vpERROR_TRACE( - "Three images are needed to compute Tsai calibration !\n"); + vpERROR_TRACE("Three images are needed to compute Tsai calibration !\n"); return -1; } } catch (...) { @@ -646,20 +625,17 @@ int vpCalibration::readData(const char *filename) if (!f.fail()) { unsigned int n; f >> n; - std::cout << "There are " << n << " point on the calibration grid " - << std::endl; + 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")); + 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; + std::cout << x << " " << y << " " << z << " " << u << " " << v << std::endl; ip.set_u(u); ip.set_v(v); addPoint(x, y, z, ip); @@ -687,8 +663,7 @@ int vpCalibration::readData(const char *filename) \return 0 if success, -1 if an error occurs. */ -int vpCalibration::readGrid(const char *filename, unsigned int &n, - std::list &oX, std::list &oY, +int vpCalibration::readGrid(const char *filename, unsigned int &n, std::list &oX, std::list &oY, std::list &oZ, bool verbose) { try { @@ -698,8 +673,7 @@ int vpCalibration::readGrid(const char *filename, unsigned int &n, f >> n; if (verbose) - std::cout << "There are " << n << " points on the calibration grid " - << std::endl; + std::cout << "There are " << n << " points on the calibration grid " << std::endl; int no_pt; double x, y, z; @@ -709,8 +683,7 @@ int vpCalibration::readGrid(const char *filename, unsigned int &n, oZ.clear(); if (n > 100000) - throw(vpException(vpException::badValue, - "Bad number of point in the calibration grid")); + 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; @@ -743,12 +716,10 @@ int vpCalibration::readGrid(const char *filename, unsigned int &n, coordinates of the data points resulting from image processing. */ -int vpCalibration::displayData(vpImage &I, vpColor color, - unsigned int thickness, int subsampling_factor) +int vpCalibration::displayData(vpImage &I, vpColor color, unsigned int thickness, int subsampling_factor) { - for (std::list::const_iterator it = Lip.begin(); - it != Lip.end(); ++it) { + for (std::list::const_iterator it = Lip.begin(); it != Lip.end(); ++it) { vpImagePoint ip = *it; if (subsampling_factor > 1.) { ip.set_u(ip.get_u() / subsampling_factor); @@ -769,8 +740,7 @@ int vpCalibration::displayData(vpImage &I, vpColor color, Admissible values are multiple of 2. Divide by this parameter the values of the camera parameters. */ -int vpCalibration::displayGrid(vpImage &I, vpColor color, - unsigned int thickness, int subsampling_factor) +int vpCalibration::displayGrid(vpImage &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; @@ -807,12 +777,9 @@ int vpCalibration::displayGrid(vpImage &I, vpColor color, // (int)vpMath::round(xp), // 5,col) ; - double cX = oX * cMo_dist[0][0] + oY * cMo_dist[0][1] + - oZ * cMo_dist[0][2] + cMo_dist[0][3]; - double cY = oX * cMo_dist[1][0] + oY * cMo_dist[1][1] + - oZ * cMo_dist[1][2] + cMo_dist[1][3]; - double cZ = oX * cMo_dist[2][0] + oY * cMo_dist[2][1] + - oZ * cMo_dist[2][2] + cMo_dist[2][3]; + double cX = oX * cMo_dist[0][0] + oY * cMo_dist[0][1] + oZ * cMo_dist[0][2] + cMo_dist[0][3]; + double cY = oX * cMo_dist[1][0] + oY * cMo_dist[1][1] + oZ * cMo_dist[1][2] + cMo_dist[1][3]; + double cZ = oX * cMo_dist[2][0] + oY * cMo_dist[2][1] + oZ * cMo_dist[2][2] + cMo_dist[2][3]; double x = cX / cZ; double y = cY / cZ; diff --git a/modules/vision/src/calibration/vpCalibrationTools.cpp b/modules/vision/src/calibration/vpCalibrationTools.cpp index a2a256cc69..5450bed15f 100644 --- a/modules/vision/src/calibration/vpCalibrationTools.cpp +++ b/modules/vision/src/calibration/vpCalibrationTools.cpp @@ -48,8 +48,7 @@ #undef MAX #undef MIN -void vpCalibration::calibLagrange(vpCameraParameters &cam_est, - vpHomogeneousMatrix &cMo_est) +void vpCalibration::calibLagrange(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est) { vpMatrix A(2 * npt, 3); @@ -177,8 +176,7 @@ void vpCalibration::calibLagrange(vpCameraParameters &cam_est, 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]); + 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 */ @@ -210,8 +208,7 @@ void vpCalibration::calibLagrange(vpCameraParameters &cam_est, this->computeStdDeviation(deviation, deviation_dist); } -void vpCalibration::calibVVS(vpCameraParameters &cam_est, - vpHomogeneousMatrix &cMo_est, bool verbose) +void vpCalibration::calibVVS(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est, bool verbose) { std::ios::fmtflags original_flags(std::cout.flags()); std::cout.precision(10); @@ -266,12 +263,9 @@ void vpCalibration::calibVVS(vpCameraParameters &cam_est, 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]; + 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]; @@ -279,8 +273,7 @@ void vpCalibration::calibVVS(vpCameraParameters &cam_est, 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]))); + r += ((vpMath::sqr(P[2 * i] - Pd[2 * i]) + vpMath::sqr(P[2 * i + 1] - Pd[2 * i + 1]))); } vpColVector error; @@ -340,18 +333,15 @@ void vpCalibration::calibVVS(vpCameraParameters &cam_est, 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]); + 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")); + 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; @@ -363,10 +353,8 @@ void vpCalibration::calibVVS(vpCameraParameters &cam_est, std::cout.flags(original_flags); } -void vpCalibration::calibVVSMulti(std::vector &table_cal, - vpCameraParameters &cam_est, - double &globalReprojectionError, - bool verbose) +void vpCalibration::calibVVSMulti(std::vector &table_cal, vpCameraParameters &cam_est, + double &globalReprojectionError, bool verbose) { std::ios::fmtflags original_flags(std::cout.flags()); std::cout.precision(10); @@ -382,8 +370,7 @@ void vpCalibration::calibVVSMulti(std::vector &table_cal, if (nbPointTotal < 4) { // vpERROR_TRACE("Not enough point to calibrate"); - throw(vpCalibrationException(vpCalibrationException::notInitializedError, - "Not enough point to calibrate")); + throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate")); } vpColVector oX(nbPointTotal), cX(nbPointTotal); @@ -442,15 +429,12 @@ void vpCalibration::calibVVSMulti(std::vector &table_cal, 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]; + 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]; @@ -458,8 +442,7 @@ void vpCalibration::calibVVSMulti(std::vector &table_cal, 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])); + r += (vpMath::sqr(P[curPoint2] - Pd[curPoint2]) + vpMath::sqr(P[curPoint2 + 1] - Pd[curPoint2 + 1])); curPoint++; } } @@ -532,9 +515,8 @@ void vpCalibration::calibVVSMulti(std::vector &table_cal, 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_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); @@ -543,18 +525,15 @@ void vpCalibration::calibVVSMulti(std::vector &table_cal, 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; + 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")); + 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; @@ -568,9 +547,7 @@ void vpCalibration::calibVVSMulti(std::vector &table_cal, std::cout.flags(original_flags); } -void vpCalibration::calibVVSWithDistortion(vpCameraParameters &cam_est, - vpHomogeneousMatrix &cMo_est, - bool verbose) +void vpCalibration::calibVVSWithDistortion(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est, bool verbose) { std::ios::fmtflags original_flags(std::cout.flags()); std::cout.precision(10); @@ -639,12 +616,9 @@ void vpCalibration::calibVVSWithDistortion(vpCameraParameters &cam_est, 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]; + 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]; @@ -693,8 +667,8 @@ void vpCalibration::calibVVSWithDistortion(vpCameraParameters &cam_est, 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])) * + 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 @@ -783,19 +757,15 @@ void vpCalibration::calibVVSWithDistortion(vpCameraParameters &cam_est, 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]); + 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")); + 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; @@ -808,9 +778,8 @@ void vpCalibration::calibVVSWithDistortion(vpCameraParameters &cam_est, std::cout.flags(original_flags); } -void vpCalibration::calibVVSWithDistortionMulti( - std::vector &table_cal, vpCameraParameters &cam_est, - double &globalReprojectionError, bool verbose) +void vpCalibration::calibVVSWithDistortionMulti(std::vector &table_cal, vpCameraParameters &cam_est, + double &globalReprojectionError, bool verbose) { std::ios::fmtflags original_flags(std::cout.flags()); std::cout.precision(10); @@ -825,8 +794,7 @@ void vpCalibration::calibVVSWithDistortionMulti( if (nbPointTotal < 4) { // vpERROR_TRACE("Not enough point to calibrate"); - throw(vpCalibrationException(vpCalibrationException::notInitializedError, - "Not enough point to calibrate")); + throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate")); } vpColVector oX(nbPointTotal), cX(nbPointTotal); @@ -876,15 +844,12 @@ void vpCalibration::calibVVSWithDistortionMulti( 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]; + 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++; } @@ -957,10 +922,8 @@ void vpCalibration::calibVVSWithDistortionMulti( 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])) * + 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; @@ -1053,27 +1016,23 @@ void vpCalibration::calibVVSWithDistortionMulti( 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]); + 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; + 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")); + vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax); + throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached")); } // double perViewError; @@ -1107,16 +1066,14 @@ void vpCalibration::calibVVSWithDistortionMulti( \param eMc : homogeneous matrix representing the transformation between the effector and the camera (output) */ -void vpCalibration::calibrationTsai( - const std::vector &cMo, - const std::vector &rMe, vpHomogeneousMatrix &eMc) +void vpCalibration::calibrationTsai(const std::vector &cMo, + const std::vector &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"); + throw vpCalibrationException(vpCalibrationException::dimensionError, "cMo and rMe have different sizes"); { vpMatrix A; vpColVector B; @@ -1142,15 +1099,13 @@ void vpCalibration::calibrationTsai( vpThetaUVector rPeij(rReij); - double theta = sqrt(rPeij[0] * rPeij[0] + rPeij[1] * rPeij[1] + - rPeij[2] * rPeij[2]); + 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]); + 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); @@ -1291,9 +1246,8 @@ void vpCalibration::calibrationTsai( } } -void vpCalibration::calibVVSMulti(unsigned int nbPose, - vpCalibration table_cal[], - vpCameraParameters &cam_est, bool verbose) +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); @@ -1309,8 +1263,7 @@ void vpCalibration::calibVVSMulti(unsigned int nbPose, if (nbPointTotal < 4) { // vpERROR_TRACE("Not enough point to calibrate"); - throw(vpCalibrationException(vpCalibrationException::notInitializedError, - "Not enough point to calibrate")); + throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate")); } vpColVector oX(nbPointTotal), cX(nbPointTotal); @@ -1369,15 +1322,12 @@ void vpCalibration::calibVVSMulti(unsigned int nbPose, 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]; + 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]; @@ -1385,8 +1335,7 @@ void vpCalibration::calibVVSMulti(unsigned int nbPose, 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])); + r += (vpMath::sqr(P[curPoint2] - Pd[curPoint2]) + vpMath::sqr(P[curPoint2 + 1] - Pd[curPoint2 + 1])); curPoint++; } } @@ -1459,9 +1408,8 @@ void vpCalibration::calibVVSMulti(unsigned int nbPose, 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_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); @@ -1470,17 +1418,14 @@ void vpCalibration::calibVVSMulti(unsigned int nbPose, 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; + 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")); + 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; @@ -1496,10 +1441,8 @@ void vpCalibration::calibVVSMulti(unsigned int nbPose, std::cout.flags(original_flags); } -void vpCalibration::calibVVSWithDistortionMulti(unsigned int nbPose, - vpCalibration table_cal[], - vpCameraParameters &cam_est, - bool verbose) +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); @@ -1514,8 +1457,7 @@ void vpCalibration::calibVVSWithDistortionMulti(unsigned int nbPose, if (nbPointTotal < 4) { // vpERROR_TRACE("Not enough point to calibrate"); - throw(vpCalibrationException(vpCalibrationException::notInitializedError, - "Not enough point to calibrate")); + throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate")); } vpColVector oX(nbPointTotal), cX(nbPointTotal); @@ -1565,15 +1507,12 @@ void vpCalibration::calibVVSWithDistortionMulti(unsigned int nbPose, 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]; + 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++; } @@ -1646,10 +1585,8 @@ void vpCalibration::calibVVSWithDistortionMulti(unsigned int nbPose, 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])) * + 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; @@ -1742,27 +1679,23 @@ void vpCalibration::calibVVSWithDistortionMulti(unsigned int nbPose, 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]); + 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; + 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")); + 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++) { diff --git a/modules/vision/src/homography-estimation/vpHomography.cpp b/modules/vision/src/homography-estimation/vpHomography.cpp index 633d8c4d48..fc5a399492 100644 --- a/modules/vision/src/homography-estimation/vpHomography.cpp +++ b/modules/vision/src/homography-estimation/vpHomography.cpp @@ -63,37 +63,29 @@ vpHomography::vpHomography() : vpArray2D(3, 3), aMb(), bP() { eye(); } \brief initialize an homography from another homography */ -vpHomography::vpHomography(const vpHomography &H) - : vpArray2D(3, 3), aMb(), bP() -{ - *this = H; -} +vpHomography::vpHomography(const vpHomography &H) : vpArray2D(3, 3), aMb(), bP() { *this = H; } /*! \brief initialize an homography from another homography */ -vpHomography::vpHomography(const vpHomogeneousMatrix &M, const vpPlane &p) - : vpArray2D(3, 3), aMb(), bP() +vpHomography::vpHomography(const vpHomogeneousMatrix &M, const vpPlane &p) : vpArray2D(3, 3), aMb(), bP() { buildFrom(M, p); } -vpHomography::vpHomography(const vpThetaUVector &tu, - const vpTranslationVector &atb, const vpPlane &p) +vpHomography::vpHomography(const vpThetaUVector &tu, const vpTranslationVector &atb, const vpPlane &p) : vpArray2D(3, 3), aMb(), bP() { buildFrom(tu, atb, p); } -vpHomography::vpHomography(const vpRotationMatrix &aRb, - const vpTranslationVector &atb, const vpPlane &p) +vpHomography::vpHomography(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &p) : vpArray2D(3, 3), aMb(), bP() { buildFrom(aRb, atb, p); } -vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p) - : vpArray2D(3, 3), aMb(), bP() +vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p) : vpArray2D(3, 3), aMb(), bP() { buildFrom(arb, p); } @@ -105,8 +97,7 @@ void vpHomography::buildFrom(const vpHomogeneousMatrix &M, const vpPlane &p) build(); } -void vpHomography::buildFrom(const vpThetaUVector &tu, - const vpTranslationVector &atb, const vpPlane &p) +void vpHomography::buildFrom(const vpThetaUVector &tu, const vpTranslationVector &atb, const vpPlane &p) { insert(tu); insert(atb); @@ -114,8 +105,7 @@ void vpHomography::buildFrom(const vpThetaUVector &tu, build(); } -void vpHomography::buildFrom(const vpRotationMatrix &aRb, - const vpTranslationVector &atb, const vpPlane &p) +void vpHomography::buildFrom(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &p) { insert(aRb); insert(atb); @@ -206,8 +196,7 @@ 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")); + throw(vpException(vpException::ioError, "Cannot write the homography to the output stream")); } } @@ -247,10 +236,8 @@ vpHomography vpHomography::operator*(const vpHomography &H) const 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())); + 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++) { @@ -333,8 +320,7 @@ vpHomography vpHomography::operator/(const double &v) const { vpHomography H; if (std::fabs(v) <= std::numeric_limits::epsilon()) - throw vpMatrixException(vpMatrixException::divideByZeroError, - "Divide by zero in method /=(double v)"); + throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)"); double vinv = 1 / v; @@ -350,8 +336,7 @@ vpHomography &vpHomography::operator/=(double v) { // if (x == 0) if (std::fabs(v) <= std::numeric_limits::epsilon()) - throw vpMatrixException(vpMatrixException::divideByZeroError, - "Divide by zero in method /=(double v)"); + throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)"); double vinv = 1 / v; @@ -386,8 +371,7 @@ vpHomography &vpHomography::operator=(const vpHomography &H) vpHomography &vpHomography::operator=(const vpMatrix &H) { if (H.getRows() != 3 || H.getCols() != 3) - throw(vpException(vpException::dimensionError, - "The matrix is not an homography")); + 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++) @@ -412,8 +396,7 @@ void vpHomography::load(std::ifstream &f) f >> (*this)[i][j]; } } else { - throw(vpException(vpException::ioError, - "Cannot read the homography from the input stream")); + throw(vpException(vpException::ioError, "Cannot read the homography from the input stream")); } } @@ -438,11 +421,9 @@ void vpHomography::build() 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 + 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++) @@ -457,8 +438,7 @@ void vpHomography::build() \f] //note d => -d verifier */ -void vpHomography::build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, - const vpPlane &bP) +void vpHomography::build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, const vpPlane &bP) { vpColVector n(3); vpColVector atb(3); @@ -472,11 +452,9 @@ void vpHomography::build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, 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 + 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++) @@ -519,9 +497,7 @@ void vpHomography::setIdentity() { eye(); } \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) +vpImagePoint vpHomography::project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa) { double xa = iPa.get_u(); double ya = iPa.get_v(); @@ -592,23 +568,17 @@ vpPoint vpHomography::project(const vpHomography &bHa, const vpPoint &Pa) \sa DLT(), ransac() */ -void vpHomography::robust(const std::vector &xb, - const std::vector &yb, - const std::vector &xa, - const std::vector &ya, vpHomography &aHb, - std::vector &inliers, double &residual, - double weights_threshold, unsigned int niter, - bool normalization) +void vpHomography::robust(const std::vector &xb, const std::vector &yb, const std::vector &xa, + const std::vector &ya, vpHomography &aHb, std::vector &inliers, + double &residual, double weights_threshold, unsigned int niter, bool normalization) { 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")); + 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")); + throw(vpException(vpException::fatalError, "There must be at least 4 matched points")); try { std::vector xan, yan, xbn, ybn; @@ -711,8 +681,7 @@ void vpHomography::robust(const std::vector &xb, if (normalization) { // H after denormalization - vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, - yg2, coef2); + vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2); } else { aHb = aHbn; } @@ -736,8 +705,7 @@ void vpHomography::robust(const std::vector &xb, residual = sqrt(residual / nbinliers); } catch (...) { - throw(vpException(vpException::fatalError, - "Cannot estimate an homography")); + throw(vpException(vpException::fatalError, "Cannot estimate an homography")); } } diff --git a/modules/vision/src/homography-estimation/vpHomographyDLT.cpp b/modules/vision/src/homography-estimation/vpHomographyDLT.cpp index 34dd76f35a..82aa35b0c5 100644 --- a/modules/vision/src/homography-estimation/vpHomographyDLT.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyDLT.cpp @@ -52,16 +52,13 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS -void vpHomography::HartleyNormalization(const std::vector &x, - const std::vector &y, - std::vector &xn, - std::vector &yn, double &xg, - double &yg, double &coef) +void vpHomography::HartleyNormalization(const std::vector &x, const std::vector &y, + std::vector &xn, std::vector &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")); + 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) @@ -105,10 +102,8 @@ void vpHomography::HartleyNormalization(const std::vector &x, } } -void vpHomography::HartleyNormalization(unsigned int n, const double *x, - const double *y, double *xn, - double *yn, double &xg, double &yg, - double &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; @@ -149,11 +144,8 @@ void vpHomography::HartleyNormalization(unsigned int n, const double *x, //--------------------------------------------------------------------------------------- -void vpHomography::HartleyDenormalization(vpHomography &aHbn, - vpHomography &aHb, double xg1, - double yg1, double coef1, - double xg2, double yg2, - double coef2) +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 @@ -255,21 +247,16 @@ void vpHomography::HartleyDenormalization(vpHomography &aHbn, \exception vpMatrixException::rankDeficient : When the rank of the matrix that should be 8 is deficient. */ -void vpHomography::DLT(const std::vector &xb, - const std::vector &yb, - const std::vector &xa, - const std::vector &ya, vpHomography &aHb, - bool normalization) +void vpHomography::DLT(const std::vector &xb, const std::vector &yb, const std::vector &xa, + const std::vector &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")); + 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")); + throw(vpException(vpException::fatalError, "There must be at least 4 matched points")); try { std::vector xan, yan, xbn, ybn; @@ -341,9 +328,7 @@ void vpHomography::DLT(const std::vector &xb, if (D[i] > 1e-7) rank++; if (rank < 7) { - throw(vpMatrixException(vpMatrixException::rankDeficient, - "Matrix rank %d is deficient (should be 8)", - rank)); + throw(vpMatrixException(vpMatrixException::rankDeficient, "Matrix rank %d is deficient (should be 8)", rank)); } // h = is the column of V associated with the smallest singular value of A @@ -367,8 +352,7 @@ void vpHomography::DLT(const std::vector &xb, if (normalization) { // H after denormalization - vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, - yg2, coef2); + vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2); } else { aHb = aHbn; } diff --git a/modules/vision/src/homography-estimation/vpHomographyExtract.cpp b/modules/vision/src/homography-estimation/vpHomographyExtract.cpp index 2c9f9e2023..728e70df4d 100644 --- a/modules/vision/src/homography-estimation/vpHomographyExtract.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyExtract.cpp @@ -58,9 +58,7 @@ const double vpHomography::sing_threshold = 0.0001; \param n : Normal vector to the plane as an output. */ -void vpHomography::computeDisplacement(vpRotationMatrix &aRb, - vpTranslationVector &atb, - vpColVector &n) +void vpHomography::computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n) { vpColVector nd(3); @@ -91,9 +89,7 @@ void vpHomography::computeDisplacement(vpRotationMatrix &aRb, \param n : Normal vector to the plane as an output. */ -void vpHomography::computeDisplacement(const vpColVector &nd, - vpRotationMatrix &aRb, - vpTranslationVector &atb, +void vpHomography::computeDisplacement(const vpColVector &nd, vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n) { computeDisplacement(*this, nd, aRb, atb, n); @@ -123,11 +119,8 @@ void vpHomography::computeDisplacement(const vpColVector &nd, \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) +void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVector &nd, vpRotationMatrix &aRb, + vpTranslationVector &atb, vpColVector &n) { /**** Declarations des variables ****/ @@ -237,8 +230,7 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, s = determinantU * determinantV; #ifdef DEBUG_Homographie - printf("s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, - s); + printf("s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s); #endif if (s < 0) mV *= -1; @@ -250,8 +242,7 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, #endif n.resize(3); - if (((sv[0] - sv[1]) < sing_threshold) && - (sv[0] - sv[2]) < sing_threshold) { + if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold) { //#ifdef DEBUG_Homographie // printf ("\nPure rotation\n"); //#endif @@ -283,11 +274,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, 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[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[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]; @@ -362,10 +351,8 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, 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]); + 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]); @@ -424,9 +411,7 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, \param n : Normal vector to the plane as an output. */ -void vpHomography::computeDisplacement(const vpHomography &aHb, - vpRotationMatrix &aRb, - vpTranslationVector &atb, +void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n) { /**** Declarations des variables ****/ @@ -538,8 +523,7 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, s = determinantU * determinantV; #ifdef DEBUG_Homographie - printf("s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, - s); + printf("s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s); #endif if (s < 0) mV *= -1; @@ -551,8 +535,7 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, #endif n.resize(3); - if (((sv[0] - sv[1]) < sing_threshold) && - (sv[0] - sv[2]) < sing_threshold) { + if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold) { //#ifdef DEBUG_Homographie // printf ("\nPure rotation\n"); //#endif @@ -584,11 +567,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, 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[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[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]; @@ -663,10 +644,8 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, 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]); + 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]); @@ -707,10 +686,8 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, #endif } -void vpHomography::computeDisplacement(const vpHomography &H, const double x, - const double y, - std::list &vR, - std::list &vT, +void vpHomography::computeDisplacement(const vpHomography &H, const double x, const double y, + std::list &vR, std::list &vT, std::list &vN) { @@ -817,19 +794,16 @@ void vpHomography::computeDisplacement(const vpHomography &H, const double x, #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]) + + 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]) + + 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); + printf("s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s); #endif // deux cas sont a traiter : @@ -888,11 +862,9 @@ void vpHomography::computeDisplacement(const vpHomography &H, const double x, 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[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])); + 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]; @@ -977,14 +949,12 @@ void vpHomography::computeDisplacement(const vpHomography &H, const double x, if (norm1ok) { // cos theta - Rprim1[0][0] = - (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]); + 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])))) / + 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]; @@ -1001,14 +971,12 @@ void vpHomography::computeDisplacement(const vpHomography &H, const double x, if (norm2ok) { // cos theta - Rprim2[0][0] = - (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]); + 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])))) / + 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]; @@ -1025,14 +993,12 @@ void vpHomography::computeDisplacement(const vpHomography &H, const double x, if (norm3ok) { // cos theta - Rprim3[0][0] = - (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]); + 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])))) / + 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]; @@ -1049,14 +1015,12 @@ void vpHomography::computeDisplacement(const vpHomography &H, const double x, if (norm4ok) { // cos theta - Rprim4[0][0] = - (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]); + 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])))) / + 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]; @@ -1116,14 +1080,12 @@ void vpHomography::computeDisplacement(const vpHomography &H, const double x, if (norm1ok) { // cos theta - Rprim1[0][0] = - (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]); + 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])))) / + 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]; @@ -1140,14 +1102,12 @@ void vpHomography::computeDisplacement(const vpHomography &H, const double x, if (norm2ok) { // cos theta - Rprim2[0][0] = - (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]); + 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])))) / + 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]; @@ -1164,14 +1124,12 @@ void vpHomography::computeDisplacement(const vpHomography &H, const double x, if (norm3ok) { // cos theta - Rprim3[0][0] = - (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]); + 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])))) / + 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]; @@ -1187,14 +1145,12 @@ void vpHomography::computeDisplacement(const vpHomography &H, const double x, if (norm4ok) { // cos theta - Rprim4[0][0] = - (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]); + 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])))) / + 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]; diff --git a/modules/vision/src/homography-estimation/vpHomographyMalis.cpp b/modules/vision/src/homography-estimation/vpHomographyMalis.cpp index f019cefd41..4ce4e76941 100644 --- a/modules/vision/src/homography-estimation/vpHomographyMalis.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyMalis.cpp @@ -86,22 +86,17 @@ const double eps = 1e-6; **************************************************************************** */ -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 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, unsigned int nbpt, double *xm, double *ym, double *xmi, double *ymi, vpMatrix &H); -void HLM(unsigned int q_cible, const std::vector &xm, - const std::vector &ym, const std::vector &xmi, - const std::vector &ymi, vpMatrix &H); +void HLM(unsigned int q_cible, const std::vector &xm, const std::vector &ym, + const std::vector &xmi, const std::vector &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) +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; /* */ @@ -188,8 +183,7 @@ void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, * DATES DE MISE A JOUR : * ****************************************************************************/ -void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, - vpMatrix &H) +void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpMatrix &H) { unsigned int i, j; @@ -264,8 +258,7 @@ void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, /** 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")); + throw(vpMatrixException(vpMatrixException::matrixError, "matrix is rank deficient")); } H.resize(3, 3); @@ -353,8 +346,7 @@ void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) if (cont_pts < 5) { // vpERROR_TRACE(" not enough point to compute the homography ... "); - throw(vpMatrixException(vpMatrixException::matrixError, - "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 ; @@ -430,24 +422,18 @@ void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) + 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]); // + 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 */ @@ -519,8 +505,7 @@ void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) 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]))) + 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; @@ -608,9 +593,8 @@ void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) } } -void HLM(unsigned int q_cible, const std::vector &xm, - const std::vector &ym, const std::vector &xmi, - const std::vector &ymi, vpMatrix &H) +void HLM(unsigned int q_cible, const std::vector &xm, const std::vector &ym, + const std::vector &xmi, const std::vector &ymi, vpMatrix &H) { unsigned int nbpt = (unsigned int)xm.size(); @@ -674,21 +658,16 @@ void HLM(unsigned int q_cible, const std::vector &xm, \sa DLT() when the scene is planar. */ -void vpHomography::HLM(const std::vector &xb, - const std::vector &yb, - const std::vector &xa, - const std::vector &ya, bool isplanar, - vpHomography &aHb) +void vpHomography::HLM(const std::vector &xb, const std::vector &yb, const std::vector &xa, + const std::vector &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")); + 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")); + 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; diff --git a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp index af959aacfe..73fee0eb54 100644 --- a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp @@ -74,8 +74,7 @@ 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) +bool vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind, double threshold_area) { unsigned int i, j, k; @@ -101,36 +100,30 @@ bool vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind, i = 0, j = 1, k = 2; - double area012 = - (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - - pa[k][0] * pa[j][1] + -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]); + double area012 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] + + -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]); i = 0; j = 1, k = 3; - double area013 = - (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - - pa[k][0] * pa[j][1] + -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]); + double area013 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] + + -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]); i = 0; j = 2, k = 3; - double area023 = - (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - - pa[k][0] * pa[j][1] + -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]); + double area023 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] + + -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]); i = 1; j = 2, k = 3; - double area123 = - (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - - pa[k][0] * pa[j][1] + -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]); + double area123 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] + + -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]); double sum_area = area012 + area013 + area023 + area123; - return ( - (sum_area < threshold_area) || - (iscolinear(pa[0], pa[1], pa[2]) || iscolinear(pa[0], pa[1], pa[3]) || - iscolinear(pa[0], pa[2], pa[3]) || iscolinear(pa[1], pa[2], pa[3]) || - iscolinear(pb[0], pb[1], pb[2]) || iscolinear(pb[0], pb[1], pb[3]) || - iscolinear(pb[0], pb[2], pb[3]) || iscolinear(pb[1], pb[2], pb[3]))); + 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 @@ -165,21 +158,16 @@ bool vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind) 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])); + 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 &xb, - const std::vector &yb, - const std::vector &xa, - const std::vector &ya) +bool vpHomography::degenerateConfiguration(const std::vector &xb, const std::vector &yb, + const std::vector &xa, const std::vector &ya) { unsigned int n = (unsigned int)xb.size(); if (n < 4) - throw(vpException(vpException::fatalError, - "There must be at least 4 matched points")); + throw(vpException(vpException::fatalError, "There must be at least 4 matched points")); std::vector pa(n), pb(n); for (unsigned i = 0; i < n; i++) { @@ -208,8 +196,7 @@ bool vpHomography::degenerateConfiguration(const std::vector &xb, return false; } // Fit model to this random selection of data points. -void vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, - vpColVector &M) +void vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M) { unsigned int n = x.getRows() / 4; std::vector xa(4), xb(4); @@ -239,8 +226,7 @@ void vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, } // Evaluate distances between points and model. -double vpHomography::computeResidual(vpColVector &x, vpColVector &M, - vpColVector &d) +double vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector &d) { unsigned int n = x.getRows() / 4; unsigned int n2 = n * 2; @@ -287,8 +273,7 @@ double vpHomography::computeResidual(vpColVector &x, vpColVector &M, } #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS -void vpHomography::initRansac(unsigned int n, double *xb, double *yb, - double *xa, double *ya, vpColVector &x) +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; @@ -330,23 +315,17 @@ void vpHomography::initRansac(unsigned int n, double *xb, double *yb, \return true if the homography could be computed, false otherwise. */ -bool vpHomography::ransac(const std::vector &xb, - const std::vector &yb, - const std::vector &xa, - const std::vector &ya, vpHomography &aHb, - std::vector &inliers, double &residual, - unsigned int nbInliersConsensus, double threshold, - bool normalization) +bool vpHomography::ransac(const std::vector &xb, const std::vector &yb, const std::vector &xa, + const std::vector &ya, vpHomography &aHb, std::vector &inliers, + double &residual, unsigned int nbInliersConsensus, double threshold, bool normalization) { 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")); + 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")); + throw(vpException(vpException::fatalError, "There must be at least 4 matched points")); vpUniRand random((const long)time(NULL)); @@ -400,10 +379,8 @@ bool vpHomography::ransac(const std::vector &xb, } try { - if (!vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, - ya_rand)) { - vpHomography::DLT(xb_rand, yb_rand, xa_rand, ya_rand, aHb, - normalization); + 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 (...) { @@ -414,8 +391,7 @@ bool vpHomography::ransac(const std::vector &xb, if (nbDegenerateIter > maxDegenerateIter) { vpERROR_TRACE("Unable to select a nondegenerate data set"); - throw(vpException(vpException::fatalError, - "Unable to select a nondegenerate data set")); + throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set")); } } @@ -495,8 +471,7 @@ bool vpHomography::ransac(const std::vector &xb, yb_best[i] = yb[best_consensus[i]]; } - vpHomography::DLT(xb_best, yb_best, xa_best, ya_best, aHb, - normalization); + vpHomography::DLT(xb_best, yb_best, xa_best, ya_best, aHb, normalization); aHb /= aHb[2][2]; residual = 0; diff --git a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp index 59f4a7600f..95c51dd5c2 100644 --- a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp @@ -89,8 +89,7 @@ static void updatePoseRotation(vpColVector &dx, vpHomogeneousMatrix &mati) mati = Delta.inverse() * mati; } -double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, - vpPoint *c2P, vpHomogeneousMatrix &c2Mc1, +double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpHomogeneousMatrix &c2Mc1, int userobust) { vpColVector e(2); @@ -113,10 +112,8 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, 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.)))) { + 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++; } } @@ -146,10 +143,8 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, 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.)))) { + 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; @@ -270,15 +265,11 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, return (W * e).sumSquare(); } -static void getPlaneInfo(vpPlane &oN, vpHomogeneousMatrix &cMo, - vpColVector &cN, double &cd) +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 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); @@ -288,11 +279,8 @@ static void getPlaneInfo(vpPlane &oN, vpHomogeneousMatrix &cMo, cd = -D1; } -double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, - vpPoint *c2P, vpPlane &oN, - vpHomogeneousMatrix &c2Mc1, - vpHomogeneousMatrix &c1Mo, - int userobust) +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; @@ -501,11 +489,8 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, return (W * e).sumSquare(); } -double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, - vpPoint *c2P, vpPlane *oN, - vpHomogeneousMatrix &c2Mc1, - vpHomogeneousMatrix &c1Mo, - int userobust) +double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpPlane *oN, + vpHomogeneousMatrix &c2Mc1, vpHomogeneousMatrix &c1Mo, int userobust) { vpColVector e(2); diff --git a/modules/vision/src/key-point/vpBasicKeyPoint.cpp b/modules/vision/src/key-point/vpBasicKeyPoint.cpp index 86e0b362fa..305ea1f9d1 100644 --- a/modules/vision/src/key-point/vpBasicKeyPoint.cpp +++ b/modules/vision/src/key-point/vpBasicKeyPoint.cpp @@ -42,7 +42,6 @@ Basic constructor. */ vpBasicKeyPoint::vpBasicKeyPoint() - : referenceImagePointsList(), currentImagePointsList(), - matchedReferencePoints(), _reference_computed(false) + : referenceImagePointsList(), currentImagePointsList(), matchedReferencePoints(), _reference_computed(false) { } diff --git a/modules/vision/src/key-point/vpFernClassifier.cpp b/modules/vision/src/key-point/vpFernClassifier.cpp index 81e55b2edb..56be5424a2 100644 --- a/modules/vision/src/key-point/vpFernClassifier.cpp +++ b/modules/vision/src/key-point/vpFernClassifier.cpp @@ -39,9 +39,8 @@ #include -#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 >= 0x020000) && \ + (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0 #include #include @@ -55,18 +54,15 @@ */ 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), + 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() + objKeypoints(), modelROI_Ref(), modelROI(), modelPoints(), imgKeypoints(), refPt(), curPt() { } @@ -81,21 +77,17 @@ vpFernClassifier::vpFernClassifier() record methods). \param _objectName : the name of the object to load */ -vpFernClassifier::vpFernClassifier(const std::string &_dataFile, - const std::string &_objectName) +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), + 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() + objKeypoints(), modelROI_Ref(), modelROI(), modelPoints(), imgKeypoints(), refPt(), curPt() { this->load(_dataFile, _objectName); } @@ -158,8 +150,7 @@ void vpFernClassifier::train() cv::Mat obj = (cv::Mat)curImg; if (this->getBlurSetting()) { - cv::GaussianBlur(obj, obj, cv::Size(getBlurSize(), getBlurSize()), - getBlurSigma(), getBlurSigma()); + cv::GaussianBlur(obj, obj, cv::Size(getBlurSize(), getBlurSize()), getBlurSigma(), getBlurSigma()); } // build pyramid @@ -175,15 +166,13 @@ void vpFernClassifier::train() 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); + 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); + vpImagePoint ip(modelPoints[i].pt.y + modelROI_Ref.y, modelPoints[i].pt.x + modelROI_Ref.x); referenceImagePointsList.push_back(ip); } @@ -204,8 +193,7 @@ void vpFernClassifier::train() \return the number of reference points. */ -unsigned int -vpFernClassifier::buildReference(const vpImage &_I) +unsigned int vpFernClassifier::buildReference(const vpImage &_I) { this->setImage(_I); @@ -233,20 +221,16 @@ vpFernClassifier::buildReference(const vpImage &_I) \return the number of reference points. */ -unsigned int vpFernClassifier::buildReference( - const vpImage &_I, const vpImagePoint &_iP, - const unsigned int _height, const unsigned int _width) +unsigned int vpFernClassifier::buildReference(const vpImage &_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()) { + 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")); + throw(vpException(vpImageException::notInTheImage, "Bad size for the subimage")); } vpImage subImage; - vpImageTools::crop(_I, (unsigned int)_iP.get_i(), (unsigned int)_iP.get_j(), - _height, _width, subImage); + vpImageTools::crop(_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 @@ -277,15 +261,12 @@ unsigned int vpFernClassifier::buildReference( \return The number of reference points. */ -unsigned int -vpFernClassifier::buildReference(const vpImage &_I, - const vpRect &_rectangle) +unsigned int vpFernClassifier::buildReference(const vpImage &_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())); + return (this->buildReference(_I, iP, (unsigned int)_rectangle.getHeight(), (unsigned int)_rectangle.getWidth())); } /*! @@ -304,9 +285,7 @@ unsigned int vpFernClassifier::matchPoint(const vpImage &_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 "); + throw vpException(vpException::notInitialized, "object is not learned, load database or build the reference "); } setImage(_I); @@ -316,9 +295,8 @@ unsigned int vpFernClassifier::matchPoint(const vpImage &_I) cv::Mat img = this->curImg; if (this->getBlurSetting()) { - cv::GaussianBlur(img, img, - cv::Size(this->getBlurSize(), this->getBlurSize()), - this->getBlurSigma(), this->getBlurSigma()); + cv::GaussianBlur(img, img, cv::Size(this->getBlurSize(), this->getBlurSize()), this->getBlurSigma(), + this->getBlurSigma()); } std::vector imgPyr; @@ -341,11 +319,8 @@ unsigned int vpFernClassifier::matchPoint(const vpImage &_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])) { + 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++; @@ -382,22 +357,17 @@ unsigned int vpFernClassifier::matchPoint(const vpImage &_I) \return the number of point which have been matched. */ -unsigned int vpFernClassifier::matchPoint(const vpImage &_I, - const vpImagePoint &_iP, - const unsigned int _height, - const unsigned int _width) +unsigned int vpFernClassifier::matchPoint(const vpImage &_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()) { + 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")); + throw(vpException(vpImageException::notInTheImage, "Bad size for the subimage")); } vpImage subImage; - vpImageTools::crop(_I, (unsigned int)_iP.get_i(), (unsigned int)_iP.get_j(), - _height, _width, subImage); + vpImageTools::crop(_I, (unsigned int)_iP.get_i(), (unsigned int)_iP.get_j(), _height, _width, subImage); return this->matchPoint(subImage); } @@ -413,14 +383,12 @@ unsigned int vpFernClassifier::matchPoint(const vpImage &_I, \return the number of point which have been matched. */ -unsigned int vpFernClassifier::matchPoint(const vpImage &_I, - const vpRect &_rectangle) +unsigned int vpFernClassifier::matchPoint(const vpImage &_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())); + return (this->matchPoint(_I, iP, (unsigned int)_rectangle.getHeight(), (unsigned int)_rectangle.getWidth())); } /*! @@ -440,16 +408,12 @@ unsigned int vpFernClassifier::matchPoint(const vpImage &_I, \param size : Size in pixels of the cross that is used to display matched points. */ -void vpFernClassifier::display(const vpImage &_Iref, - const vpImage &_Icurrent, +void vpFernClassifier::display(const vpImage &_Iref, const vpImage &_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); + vpDisplay::displayCross(_Iref, referenceImagePointsList[matchedReferencePoints[i]], size, vpColor::red); + vpDisplay::displayCross(_Icurrent, currentImagePointsList[i], size, vpColor::green); } } @@ -464,12 +428,10 @@ void vpFernClassifier::display(const vpImage &_Iref, \param color : Color used to display the matched points. */ -void vpFernClassifier::display(const vpImage &_Icurrent, - unsigned int size, const vpColor &color) +void vpFernClassifier::display(const vpImage &_Icurrent, unsigned int size, const vpColor &color) { for (unsigned int i = 0; i < matchedReferencePoints.size(); i++) { - vpDisplay::displayCross(_Icurrent, currentImagePointsList[i], size, - color); + vpDisplay::displayCross(_Icurrent, currentImagePointsList[i], size, color); } } @@ -484,8 +446,7 @@ void vpFernClassifier::display(const vpImage &_Icurrent, \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*/) +void vpFernClassifier::load(const std::string &_dataFile, const std::string & /*_objectName*/) { std::cout << " > Load data for the planar object detector..." << std::endl; @@ -514,8 +475,7 @@ void vpFernClassifier::load(const std::string &_dataFile, \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) +void vpFernClassifier::record(const std::string &_objectName, const std::string &_dataFile) { /* part of code from OpenCV planarObjectDetector */ cv::FileStorage fs(_dataFile, cv::FileStorage::WRITE); @@ -557,13 +517,11 @@ void vpFernClassifier::setImage(const vpImage &I) curImg = NULL; } if ((I.getWidth() % 8) == 0) { - curImg = cvCreateImageHeader( - cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1); + 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."); + throw vpException(vpException::memoryAllocationError, "Could not create the image in the OpenCV format."); } } else { vpImageConvert::convert(I, curImg); diff --git a/modules/vision/src/key-point/vpKeyPoint.cpp b/modules/vision/src/key-point/vpKeyPoint.cpp index d2512975b1..15a75b9e28 100644 --- a/modules/vision/src/key-point/vpKeyPoint.cpp +++ b/modules/vision/src/key-point/vpKeyPoint.cpp @@ -63,22 +63,17 @@ #else #error Unknown machine endianness detected. #endif -#elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN) || \ - defined(__BIG_ENDIAN__) && !defined(__LITTLE_ENDIAN__) +#elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN) || defined(__BIG_ENDIAN__) && !defined(__LITTLE_ENDIAN__) #define VISP_BIG_ENDIAN -#elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN) || \ - defined(__LITTLE_ENDIAN__) && !defined(__BIG_ENDIAN__) +#elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN) || defined(__LITTLE_ENDIAN__) && !defined(__BIG_ENDIAN__) #define VISP_LITTLE_ENDIAN -#elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || \ - defined(__powerpc__) || defined(__ppc__) || defined(__hpux) || \ - defined(_MIPSEB) || defined(_POWER) || defined(__s390__) +#elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || defined(__powerpc__) || defined(__ppc__) || \ + defined(__hpux) || defined(_MIPSEB) || defined(_POWER) || defined(__s390__) #define VISP_BIG_ENDIAN -#elif defined(__i386__) || defined(__alpha__) || defined(__ia64) || \ - defined(__ia64__) || defined(_M_IX86) || defined(_M_IA64) || \ - defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) || \ - defined(_M_AMD64) || defined(__x86_64) || defined(__x86_64__) || \ - defined(_M_X64) +#elif defined(__i386__) || defined(__alpha__) || defined(__ia64) || defined(__ia64__) || defined(_M_IX86) || \ + defined(_M_IA64) || defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) || defined(_M_AMD64) || \ + defined(__x86_64) || defined(__x86_64__) || defined(_M_X64) #define VISP_LITTLE_ENDIAN #else @@ -110,8 +105,7 @@ inline cv::DMatch knnToDMatch(const std::vector &knnMatches) // \param match : Point to convert in ViSP type. // \return The train index. // */ -inline vpImagePoint -matchRansacToVpImage(const std::pair &pair) +inline vpImagePoint matchRansacToVpImage(const std::pair &pair) { return vpImagePoint(pair.first.pt.y, pair.first.pt.x); } @@ -129,17 +123,14 @@ matchRansacToVpImage(const std::pair &pair) #ifdef VISP_BIG_ENDIAN // Swap 16 bits by shifting to the right the first byte and by shifting to the // left the second byte -uint16_t swap16bits(const uint16_t val) -{ - return (((val >> 8) & 0x00FF) | ((val << 8) & 0xFF00)); -} +uint16_t swap16bits(const uint16_t val) { return (((val >> 8) & 0x00FF) | ((val << 8) & 0xFF00)); } // Swap 32 bits by shifting to the right the first 2 bytes and by shifting to // the left the last 2 bytes uint32_t swap32bits(const uint32_t val) { - return (((val >> 24) & 0x000000FF) | ((val >> 8) & 0x0000FF00) | - ((val << 8) & 0x00FF0000) | ((val << 24) & 0xFF000000)); + return (((val >> 24) & 0x000000FF) | ((val >> 8) & 0x0000FF00) | ((val << 8) & 0x00FF0000) | + ((val << 24) & 0xFF000000)); } // Swap a float, the union is necessary because of the representation of a @@ -246,8 +237,7 @@ void readBinaryDoubleLE(std::ifstream &file, double &double_value) } // Write an unsigned short in little endian -void writeBinaryUShortLE(std::ofstream &file, - const unsigned short ushort_value) +void writeBinaryUShortLE(std::ofstream &file, const unsigned short ushort_value) { #ifdef VISP_BIG_ENDIAN // Swap bytes order to little endian @@ -322,32 +312,22 @@ void writeBinaryDoubleLE(std::ofstream &file, const double double_value) \param matcherName : Name of the matcher. \param filterType : Filtering matching method chosen. */ -vpKeyPoint::vpKeyPoint(const vpFeatureDetectorType &detectorType, - const vpFeatureDescriptorType &descriptorType, - 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) +vpKeyPoint::vpKeyPoint(const vpFeatureDetectorType &detectorType, const vpFeatureDescriptorType &descriptorType, + 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_useConsensusPercentage(false), m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true) { initFeatureNames(); @@ -367,32 +347,22 @@ vpKeyPoint::vpKeyPoint(const vpFeatureDetectorType &detectorType, \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) +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_useConsensusPercentage(false), m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true) { initFeatureNames(); @@ -412,32 +382,22 @@ vpKeyPoint::vpKeyPoint(const std::string &detectorName, multiple extractors. \param matcherName : Name of the matcher. \param filterType : Filtering matching method chosen. */ -vpKeyPoint::vpKeyPoint(const std::vector &detectorNames, - const std::vector &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) +vpKeyPoint::vpKeyPoint(const std::vector &detectorNames, const std::vector &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_useConsensusPercentage(false), m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true) { initFeatureNames(); @@ -452,8 +412,7 @@ vpKeyPoint::vpKeyPoint(const std::vector &detectorNames, \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) +void vpKeyPoint::affineSkew(double tilt, double phi, cv::Mat &img, cv::Mat &mask, cv::Mat &Ai) { int h = img.rows; int w = img.cols; @@ -483,8 +442,7 @@ void vpKeyPoint::affineSkew(double tilt, double phi, cv::Mat &img, cv::Rect rect = cv::boundingRect(tcorners); A = (cv::Mat_(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); + 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::epsilon()) { @@ -509,10 +467,7 @@ void vpKeyPoint::affineSkew(double tilt, double phi, cv::Mat &img, \param I : Input reference image. \return The number of detected keypoints in the image \p I. */ -unsigned int vpKeyPoint::buildReference(const vpImage &I) -{ - return buildReference(I, vpRect()); -} +unsigned int vpKeyPoint::buildReference(const vpImage &I) { return buildReference(I, vpRect()); } /*! Build the reference keypoints list in a region of interest in the image. @@ -523,10 +478,8 @@ unsigned int vpKeyPoint::buildReference(const vpImage &I) \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 &I, - const vpImagePoint &iP, - const unsigned int height, - const unsigned int width) +unsigned int vpKeyPoint::buildReference(const vpImage &I, const vpImagePoint &iP, + const unsigned int height, const unsigned int width) { return buildReference(I, vpRect(iP, width, height)); @@ -539,8 +492,7 @@ unsigned int vpKeyPoint::buildReference(const vpImage &I, \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 &I, - const vpRect &rectangle) +unsigned int vpKeyPoint::buildReference(const vpImage &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 @@ -558,16 +510,14 @@ unsigned int vpKeyPoint::buildReference(const vpImage &I, // Flatten the different train lists m_trainKeyPoints.clear(); - for (std::vector >::const_iterator it = - listOfTrainKeyPoints.begin(); + for (std::vector >::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::const_iterator it = - listOfTrainDescriptors.begin(); - it != listOfTrainDescriptors.end(); ++it) { + for (std::vector::const_iterator it = listOfTrainDescriptors.begin(); it != listOfTrainDescriptors.end(); + ++it) { if (first) { first = false; it->copyTo(m_trainDescriptors); @@ -582,9 +532,7 @@ unsigned int vpKeyPoint::buildReference(const vpImage &I, // 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::const_iterator it = - m_trainKeyPoints.begin(); - it != m_trainKeyPoints.end(); ++it) { + for (std::vector::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) { m_mapOfImageId[it->class_id] = m_currentImageId; } @@ -614,10 +562,8 @@ unsigned int vpKeyPoint::buildReference(const vpImage &I, 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 &I, - std::vector &trainKeyPoints, - std::vector &points3f, - const bool append, const int class_id) +void vpKeyPoint::buildReference(const vpImage &I, std::vector &trainKeyPoints, + std::vector &points3f, const bool append, const int class_id) { cv::Mat trainDescriptors; // Copy the input list of keypoints @@ -631,20 +577,15 @@ void vpKeyPoint::buildReference(const vpImage &I, // as the value std::map mapOfKeypointHashes; size_t cpt = 0; - for (std::vector::const_iterator - it = trainKeyPoints_tmp.begin(); - it != trainKeyPoints_tmp.end(); ++it, cpt++) { + for (std::vector::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); + ++it, cpt++) { mapOfKeypointHashes[myKeypointHash(*it)] = cpt; } std::vector trainPoints_tmp; - for (std::vector::const_iterator it = - trainKeyPoints.begin(); - it != trainKeyPoints.end(); ++it) { - if (mapOfKeypointHashes.find(myKeypointHash(*it)) != - mapOfKeypointHashes.end()) { - trainPoints_tmp.push_back( - points3f[mapOfKeypointHashes[myKeypointHash(*it)]]); + for (std::vector::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) { + if (mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) { + trainPoints_tmp.push_back(points3f[mapOfKeypointHashes[myKeypointHash(*it)]]); } } @@ -652,8 +593,7 @@ void vpKeyPoint::buildReference(const vpImage &I, points3f = trainPoints_tmp; } - buildReference(I, trainKeyPoints, trainDescriptors, points3f, append, - class_id); + buildReference(I, trainKeyPoints, trainDescriptors, points3f, append, class_id); } /*! @@ -668,11 +608,9 @@ void vpKeyPoint::buildReference(const vpImage &I, present. \param class_id : The class id to be set to the input cv::KeyPoint if != -1. */ -void vpKeyPoint::buildReference( - const vpImage &I, - const std::vector &trainKeyPoints, - const cv::Mat &trainDescriptors, const std::vector &points3f, - const bool append, const int class_id) +void vpKeyPoint::buildReference(const vpImage &I, const std::vector &trainKeyPoints, + const cv::Mat &trainDescriptors, const std::vector &points3f, + const bool append, const int class_id) { if (!append) { m_currentImageId = 0; @@ -687,17 +625,15 @@ void vpKeyPoint::buildReference( std::vector trainKeyPoints_tmp = trainKeyPoints; // Set class_id if != -1 if (class_id != -1) { - for (std::vector::iterator it = trainKeyPoints_tmp.begin(); - it != trainKeyPoints_tmp.end(); ++it) { + for (std::vector::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::const_iterator it = - trainKeyPoints_tmp.begin(); - it != trainKeyPoints_tmp.end(); ++it) { + for (std::vector::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); + ++it) { m_mapOfImageId[it->class_id] = m_currentImageId; } @@ -705,20 +641,16 @@ void vpKeyPoint::buildReference( m_mapOfImages[m_currentImageId] = I; // Append reference lists - this->m_trainKeyPoints.insert(this->m_trainKeyPoints.end(), - trainKeyPoints_tmp.begin(), - trainKeyPoints_tmp.end()); + 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()); + 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_trainKeyPoints, referenceImagePointsList); vpConvert::convertFromOpenCV(this->m_trainPoints, m_trainVpPoints); // Add train descriptors in matcher object @@ -743,10 +675,8 @@ void vpKeyPoint::buildReference( between the world and the camera frames. \param point : 3D coordinate in the world/object frame computed. */ -void vpKeyPoint::compute3D(const cv::KeyPoint &candidate, - const std::vector &roi, - const vpCameraParameters &cam, - const vpHomogeneousMatrix &cMo, cv::Point3f &point) +void vpKeyPoint::compute3D(const cv::KeyPoint &candidate, const std::vector &roi, + const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point) { /* compute plane equation */ std::vector::const_iterator it_roi = roi.begin(); @@ -758,8 +688,7 @@ void vpKeyPoint::compute3D(const cv::KeyPoint &candidate, 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); + 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; @@ -770,8 +699,7 @@ void vpKeyPoint::compute3D(const cv::KeyPoint &candidate, 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]); + point = cv::Point3f((float)point_obj[0], (float)point_obj[1], (float)point_obj[2]); } /*! @@ -789,10 +717,8 @@ void vpKeyPoint::compute3D(const cv::KeyPoint &candidate, between the world and the camera frames. \param point : 3D coordinate in the world/object frame computed. */ -void vpKeyPoint::compute3D(const vpImagePoint &candidate, - const std::vector &roi, - const vpCameraParameters &cam, - const vpHomogeneousMatrix &cMo, vpPoint &point) +void vpKeyPoint::compute3D(const vpImagePoint &candidate, const std::vector &roi, + const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, vpPoint &point) { /* compute plane equation */ std::vector::const_iterator it_roi = roi.begin(); @@ -833,12 +759,11 @@ void vpKeyPoint::compute3D(const vpImagePoint &candidate, the world/object frame) 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 &candidates, - const std::vector &polygons, - const std::vector > &roisPt, - std::vector &points, cv::Mat *descriptors) +void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + std::vector &candidates, + const std::vector &polygons, + const std::vector > &roisPt, + std::vector &points, cv::Mat *descriptors) { std::vector candidatesToCheck = candidates; @@ -848,19 +773,15 @@ void vpKeyPoint::compute3DForPointsInPolygons( cv::Point3f pt; cv::Mat desc; - std::vector > pairOfCandidatesToCheck( - candidatesToCheck.size()); + std::vector > pairOfCandidatesToCheck(candidatesToCheck.size()); for (size_t i = 0; i < candidatesToCheck.size(); i++) { - pairOfCandidatesToCheck[i] = - std::pair(candidatesToCheck[i], i); + pairOfCandidatesToCheck[i] = std::pair(candidatesToCheck[i], i); } size_t cpt1 = 0; std::vector polygons_tmp = polygons; - for (std::vector::iterator it1 = polygons_tmp.begin(); - it1 != polygons_tmp.end(); ++it1, cpt1++) { - std::vector >::iterator it2 = - pairOfCandidatesToCheck.begin(); + for (std::vector::iterator it1 = polygons_tmp.begin(); it1 != polygons_tmp.end(); ++it1, cpt1++) { + std::vector >::iterator it2 = pairOfCandidatesToCheck.begin(); while (it2 != pairOfCandidatesToCheck.end()) { imPt.set_ij(it2->first.pt.y, it2->first.pt.x); @@ -902,12 +823,11 @@ void vpKeyPoint::compute3DForPointsInPolygons( faces. \param descriptors : Optional parameter, pointer to the descriptors to filter */ -void vpKeyPoint::compute3DForPointsInPolygons( - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - std::vector &candidates, - const std::vector &polygons, - const std::vector > &roisPt, - std::vector &points, cv::Mat *descriptors) +void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, + std::vector &candidates, + const std::vector &polygons, + const std::vector > &roisPt, + std::vector &points, cv::Mat *descriptors) { std::vector candidatesToCheck = candidates; @@ -916,19 +836,15 @@ void vpKeyPoint::compute3DForPointsInPolygons( vpPoint pt; cv::Mat desc; - std::vector > pairOfCandidatesToCheck( - candidatesToCheck.size()); + std::vector > pairOfCandidatesToCheck(candidatesToCheck.size()); for (size_t i = 0; i < candidatesToCheck.size(); i++) { - pairOfCandidatesToCheck[i] = - std::pair(candidatesToCheck[i], i); + pairOfCandidatesToCheck[i] = std::pair(candidatesToCheck[i], i); } size_t cpt1 = 0; std::vector polygons_tmp = polygons; - for (std::vector::iterator it1 = polygons_tmp.begin(); - it1 != polygons_tmp.end(); ++it1, cpt1++) { - std::vector >::iterator it2 = - pairOfCandidatesToCheck.begin(); + for (std::vector::iterator it1 = polygons_tmp.begin(); it1 != polygons_tmp.end(); ++it1, cpt1++) { + std::vector >::iterator it2 = pairOfCandidatesToCheck.begin(); while (it2 != pairOfCandidatesToCheck.end()) { if (it1->isInside(it2->first)) { @@ -965,12 +881,10 @@ void vpKeyPoint::compute3DForPointsInPolygons( pointer to the descriptors to filter. */ void vpKeyPoint::compute3DForPointsOnCylinders( - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - std::vector &candidates, + const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, std::vector &candidates, const std::vector &cylinders, - const std::vector > > - &vectorOfCylinderRois, - std::vector &points, cv::Mat *descriptors) + const std::vector > > &vectorOfCylinderRois, std::vector &points, + cv::Mat *descriptors) { std::vector candidatesToCheck = candidates; candidates.clear(); @@ -979,32 +893,26 @@ void vpKeyPoint::compute3DForPointsOnCylinders( // Keep only keypoints on cylinders size_t cpt_keypoint = 0; - for (std::vector::const_iterator - it1 = candidatesToCheck.begin(); - it1 != candidatesToCheck.end(); ++it1, cpt_keypoint++) { + for (std::vector::const_iterator it1 = candidatesToCheck.begin(); it1 != candidatesToCheck.end(); + ++it1, cpt_keypoint++) { size_t cpt_cylinder = 0; // Iterate through the list of vpCylinders - for (std::vector > >::const_iterator - it2 = vectorOfCylinderRois.begin(); + for (std::vector > >::const_iterator it2 = vectorOfCylinderRois.begin(); it2 != vectorOfCylinderRois.end(); ++it2, cpt_cylinder++) { // Iterate through the list of the bounding boxes of the current // vpCylinder - for (std::vector >::const_iterator it3 = - it2->begin(); - it3 != it2->end(); ++it3) { + for (std::vector >::const_iterator it3 = it2->begin(); it3 != it2->end(); ++it3) { if (vpPolygon::isInside(*it3, it1->pt.y, it1->pt.x)) { candidates.push_back(*it1); // Calculate the 3D coordinates for each keypoint located on // cylinders double xm = 0.0, ym = 0.0; - vpPixelMeterConversion::convertPoint(cam, it1->pt.x, it1->pt.y, xm, - ym); + vpPixelMeterConversion::convertPoint(cam, it1->pt.x, it1->pt.y, xm, ym); double Z = cylinders[cpt_cylinder].computeZ(xm, ym); - if (!vpMath::isNaN(Z) && - Z > std::numeric_limits::epsilon()) { + if (!vpMath::isNaN(Z) && Z > std::numeric_limits::epsilon()) { vpColVector point_cam(4); point_cam[0] = xm * Z; point_cam[1] = ym * Z; @@ -1014,8 +922,7 @@ void vpKeyPoint::compute3DForPointsOnCylinders( point_obj = cMo.inverse() * point_cam; vpPoint pt; pt.setWorldCoordinates(point_obj); - points.push_back(cv::Point3f( - (float)pt.get_oX(), (float)pt.get_oY(), (float)pt.get_oZ())); + points.push_back(cv::Point3f((float)pt.get_oX(), (float)pt.get_oY(), (float)pt.get_oZ())); if (descriptors != NULL) { desc.push_back(descriptors->row((int)cpt_keypoint)); @@ -1049,12 +956,10 @@ void vpKeyPoint::compute3DForPointsOnCylinders( parameter, pointer to the descriptors to filter. */ void vpKeyPoint::compute3DForPointsOnCylinders( - const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, - std::vector &candidates, + const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, std::vector &candidates, const std::vector &cylinders, - const std::vector > > - &vectorOfCylinderRois, - std::vector &points, cv::Mat *descriptors) + const std::vector > > &vectorOfCylinderRois, std::vector &points, + cv::Mat *descriptors) { std::vector candidatesToCheck = candidates; candidates.clear(); @@ -1063,32 +968,26 @@ void vpKeyPoint::compute3DForPointsOnCylinders( // Keep only keypoints on cylinders size_t cpt_keypoint = 0; - for (std::vector::const_iterator - it1 = candidatesToCheck.begin(); - it1 != candidatesToCheck.end(); ++it1, cpt_keypoint++) { + for (std::vector::const_iterator it1 = candidatesToCheck.begin(); it1 != candidatesToCheck.end(); + ++it1, cpt_keypoint++) { size_t cpt_cylinder = 0; // Iterate through the list of vpCylinders - for (std::vector > >::const_iterator - it2 = vectorOfCylinderRois.begin(); + for (std::vector > >::const_iterator it2 = vectorOfCylinderRois.begin(); it2 != vectorOfCylinderRois.end(); ++it2, cpt_cylinder++) { // Iterate through the list of the bounding boxes of the current // vpCylinder - for (std::vector >::const_iterator it3 = - it2->begin(); - it3 != it2->end(); ++it3) { + for (std::vector >::const_iterator it3 = it2->begin(); it3 != it2->end(); ++it3) { if (vpPolygon::isInside(*it3, it1->get_i(), it1->get_j())) { candidates.push_back(*it1); // Calculate the 3D coordinates for each keypoint located on // cylinders double xm = 0.0, ym = 0.0; - vpPixelMeterConversion::convertPoint(cam, it1->get_u(), - it1->get_v(), xm, ym); + vpPixelMeterConversion::convertPoint(cam, it1->get_u(), it1->get_v(), xm, ym); double Z = cylinders[cpt_cylinder].computeZ(xm, ym); - if (!vpMath::isNaN(Z) && - Z > std::numeric_limits::epsilon()) { + if (!vpMath::isNaN(Z) && Z > std::numeric_limits::epsilon()) { vpColVector point_cam(4); point_cam[0] = xm * Z; point_cam[1] = ym * Z; @@ -1129,18 +1028,13 @@ void vpKeyPoint::compute3DForPointsOnCylinders( 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 &imagePoints, - const std::vector &objectPoints, - const vpCameraParameters &cam, - vpHomogeneousMatrix &cMo, - std::vector &inlierIndex, - double &elapsedTime, - bool (*func)(vpHomogeneousMatrix *)) +bool vpKeyPoint::computePose(const std::vector &imagePoints, const std::vector &objectPoints, + const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector &inlierIndex, + double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) { double t = vpTime::measureTimeMs(); - if (imagePoints.size() < 4 || objectPoints.size() < 4 || - imagePoints.size() != objectPoints.size()) { + 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)." @@ -1150,14 +1044,13 @@ bool vpKeyPoint::computePose(const std::vector &imagePoints, } cv::Mat cameraMatrix = - (cv::Mat_(3, 3) << cam.get_px(), 0, cam.get_u0(), 0, - cam.get_py(), cam.get_v0(), 0, 0, 1); + (cv::Mat_(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 + // 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); @@ -1165,18 +1058,17 @@ bool vpKeyPoint::computePose(const std::vector &imagePoints, 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, + 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 +// 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 @@ -1185,31 +1077,26 @@ bool vpKeyPoint::computePose(const std::vector &imagePoints, // 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. +// 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()); + 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); + 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(0), tvec.at(1), - tvec.at(2)); - vpThetaUVector thetaUVector(rvec.at(0), rvec.at(1), - rvec.at(2)); + vpTranslationVector translationVec(tvec.at(0), tvec.at(1), tvec.at(2)); + vpThetaUVector thetaUVector(rvec.at(0), rvec.at(1), rvec.at(2)); cMo = vpHomogeneousMatrix(translationVec, thetaUVector); if (func != NULL) { @@ -1237,15 +1124,11 @@ bool vpKeyPoint::computePose(const std::vector &imagePoints, 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 &objectVpPoints, - vpHomogeneousMatrix &cMo, - std::vector &inliers, - double &elapsedTime, - bool (*func)(vpHomogeneousMatrix *)) +bool vpKeyPoint::computePose(const std::vector &objectVpPoints, vpHomogeneousMatrix &cMo, + std::vector &inliers, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) { std::vector inlierIndex; - return computePose(objectVpPoints, cMo, inliers, inlierIndex, elapsedTime, - func); + return computePose(objectVpPoints, cMo, inliers, inlierIndex, elapsedTime, func); } /*! @@ -1262,11 +1145,8 @@ bool vpKeyPoint::computePose(const std::vector &objectVpPoints, 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 &objectVpPoints, - vpHomogeneousMatrix &cMo, - std::vector &inliers, - std::vector &inlierIndex, - double &elapsedTime, +bool vpKeyPoint::computePose(const std::vector &objectVpPoints, vpHomogeneousMatrix &cMo, + std::vector &inliers, std::vector &inlierIndex, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) { double t = vpTime::measureTimeMs(); @@ -1281,17 +1161,14 @@ bool vpKeyPoint::computePose(const std::vector &objectVpPoints, vpPose pose; - for (std::vector::const_iterator it = objectVpPoints.begin(); - it != objectVpPoints.end(); ++it) { + for (std::vector::const_iterator it = objectVpPoints.begin(); it != objectVpPoints.end(); ++it) { pose.addPoint(*it); } - unsigned int nbInlierToReachConsensus = - (unsigned int)m_nbRansacMinInlierCount; + unsigned int nbInlierToReachConsensus = (unsigned int)m_nbRansacMinInlierCount; if (m_useConsensusPercentage) { nbInlierToReachConsensus = - (unsigned int)(m_ransacConsensusPercentage / 100.0 * - (double)m_queryFilteredKeyPoints.size()); + (unsigned int)(m_ransacConsensusPercentage / 100.0 * (double)m_queryFilteredKeyPoints.size()); } pose.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus); @@ -1343,9 +1220,8 @@ bool vpKeyPoint::computePose(const std::vector &objectVpPoints, detected keypoints and the location of the projection of the 3D model with the estimated pose. */ -double vpKeyPoint::computePoseEstimationError( - const std::vector > &matchKeyPoints, - const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est) +double vpKeyPoint::computePoseEstimationError(const std::vector > &matchKeyPoints, + const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est) { if (matchKeyPoints.size() == 0) { // return std::numeric_limits::max(); // create an error under @@ -1356,8 +1232,7 @@ double vpKeyPoint::computePoseEstimationError( std::vector errors(matchKeyPoints.size()); size_t cpt = 0; vpPoint pt; - for (std::vector >::const_iterator - it = matchKeyPoints.begin(); + for (std::vector >::const_iterator it = matchKeyPoints.begin(); it != matchKeyPoints.end(); ++it, cpt++) { pt.set_oX(it->second.x); pt.set_oY(it->second.y); @@ -1365,8 +1240,7 @@ double vpKeyPoint::computePoseEstimationError( 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)); + 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(); @@ -1380,8 +1254,7 @@ double vpKeyPoint::computePoseEstimationError( \param ICurrent : Current image. \param IMatching : Image matching. */ -void vpKeyPoint::createImageMatching(vpImage &IRef, - vpImage &ICurrent, +void vpKeyPoint::createImageMatching(vpImage &IRef, vpImage &ICurrent, vpImage &IMatching) { // Image matching side by side @@ -1400,8 +1273,7 @@ void vpKeyPoint::createImageMatching(vpImage &IRef, \param ICurrent : Current image. \param IMatching : Image initialized with appropriate size. */ -void vpKeyPoint::createImageMatching(vpImage &ICurrent, - vpImage &IMatching) +void vpKeyPoint::createImageMatching(vpImage &ICurrent, vpImage &IMatching) { // Nb images in the training database + the current image we want to detect // the object @@ -1418,8 +1290,7 @@ void vpKeyPoint::createImageMatching(vpImage &ICurrent, } 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)); + unsigned int nbImgSqrt = (unsigned int)vpMath::round(std::sqrt((double)nbImg)); // Number of columns in the mosaic grid unsigned int nbWidth = nbImgSqrt; @@ -1433,9 +1304,8 @@ void vpKeyPoint::createImageMatching(vpImage &ICurrent, unsigned int maxW = ICurrent.getWidth(); unsigned int maxH = ICurrent.getHeight(); - for (std::map >::const_iterator it = - m_mapOfImages.begin(); - it != m_mapOfImages.end(); ++it) { + for (std::map >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); + ++it) { if (maxW < it->second.getWidth()) { maxW = it->second.getWidth(); } @@ -1456,9 +1326,7 @@ void vpKeyPoint::createImageMatching(vpImage &ICurrent, \param keyPoints : Output list of the detected keypoints. \param rectangle : Optional rectangle of the region of interest. */ -void vpKeyPoint::detect(const vpImage &I, - std::vector &keyPoints, - const vpRect &rectangle) +void vpKeyPoint::detect(const vpImage &I, std::vector &keyPoints, const vpRect &rectangle) { double elapsedTime; detect(I, keyPoints, elapsedTime, rectangle); @@ -1472,9 +1340,7 @@ void vpKeyPoint::detect(const vpImage &I, \param mask : Optional 8-bit integer mask to detect only where mask[i][j] != 0. */ -void vpKeyPoint::detect(const cv::Mat &matImg, - std::vector &keyPoints, - const cv::Mat &mask) +void vpKeyPoint::detect(const cv::Mat &matImg, std::vector &keyPoints, const cv::Mat &mask) { double elapsedTime; detect(matImg, keyPoints, elapsedTime, mask); @@ -1488,9 +1354,8 @@ void vpKeyPoint::detect(const cv::Mat &matImg, \param elapsedTime : Elapsed time. \param rectangle : Optional rectangle of the region of interest. */ -void vpKeyPoint::detect(const vpImage &I, - std::vector &keyPoints, - double &elapsedTime, const vpRect &rectangle) +void vpKeyPoint::detect(const vpImage &I, std::vector &keyPoints, double &elapsedTime, + const vpRect &rectangle) { cv::Mat matImg; vpImageConvert::convert(I, matImg, false); @@ -1516,15 +1381,13 @@ void vpKeyPoint::detect(const vpImage &I, \param mask : Optional 8-bit integer mask to detect only where mask[i][j] != 0. */ -void vpKeyPoint::detect(const cv::Mat &matImg, - std::vector &keyPoints, - double &elapsedTime, const cv::Mat &mask) +void vpKeyPoint::detect(const cv::Mat &matImg, std::vector &keyPoints, double &elapsedTime, + const cv::Mat &mask) { double t = vpTime::measureTimeMs(); keyPoints.clear(); - for (std::map >::const_iterator - it = m_detectors.begin(); + for (std::map >::const_iterator it = m_detectors.begin(); it != m_detectors.end(); ++it) { std::vector kp; it->second->detect(matImg, kp, mask); @@ -1541,23 +1404,16 @@ void vpKeyPoint::detect(const cv::Mat &matImg, \param ICurrent : Input current image. \param size : Size of the displayed cross. */ -void vpKeyPoint::display(const vpImage &IRef, - const vpImage &ICurrent, - unsigned int size) +void vpKeyPoint::display(const vpImage &IRef, const vpImage &ICurrent, unsigned int size) { std::vector vpQueryImageKeyPoints; getQueryKeyPoints(vpQueryImageKeyPoints); std::vector vpTrainImageKeyPoints; getTrainKeyPoints(vpTrainImageKeyPoints); - for (std::vector::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); + for (std::vector::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); } } @@ -1568,16 +1424,13 @@ void vpKeyPoint::display(const vpImage &IRef, \param size : Size of the displayed crosses. \param color : Color of the crosses. */ -void vpKeyPoint::display(const vpImage &ICurrent, - unsigned int size, const vpColor &color) +void vpKeyPoint::display(const vpImage &ICurrent, unsigned int size, const vpColor &color) { std::vector vpQueryImageKeyPoints; getQueryKeyPoints(vpQueryImageKeyPoints); - for (std::vector::const_iterator it = m_filteredMatches.begin(); - it != m_filteredMatches.end(); ++it) { - vpDisplay::displayCross( - ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, color); + for (std::vector::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) { + vpDisplay::displayCross(ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, color); } } @@ -1592,11 +1445,8 @@ void vpKeyPoint::display(const vpImage &ICurrent, \param color : Color to use, if none, we pick randomly a color for each pair of matching. */ -void vpKeyPoint::displayMatching(const vpImage &IRef, - vpImage &IMatching, - unsigned int crossSize, - unsigned int lineThickness, - const vpColor &color) +void vpKeyPoint::displayMatching(const vpImage &IRef, vpImage &IMatching, + unsigned int crossSize, unsigned int lineThickness, const vpColor &color) { bool randomColor = (color == vpColor::none); srand((unsigned int)time(NULL)); @@ -1608,20 +1458,17 @@ void vpKeyPoint::displayMatching(const vpImage &IRef, getTrainKeyPoints(trainImageKeyPoints); vpImagePoint leftPt, rightPt; - for (std::vector::const_iterator it = m_filteredMatches.begin(); - it != m_filteredMatches.end(); ++it) { + for (std::vector::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()); + 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); + vpDisplay::displayLine(IMatching, leftPt, rightPt, currentColor, lineThickness); } } @@ -1636,10 +1483,9 @@ void vpKeyPoint::displayMatching(const vpImage &IRef, available. \param crossSize : Size of the displayed crosses. \param lineThickness : Thickness of the displayed line. */ -void vpKeyPoint::displayMatching( - const vpImage &ICurrent, vpImage &IMatching, - const std::vector &ransacInliers, unsigned int crossSize, - unsigned int lineThickness) +void vpKeyPoint::displayMatching(const vpImage &ICurrent, vpImage &IMatching, + const std::vector &ransacInliers, unsigned int crossSize, + unsigned int lineThickness) { if (m_mapOfImages.empty() || m_mapOfImageId.empty()) { // No training images so return @@ -1656,8 +1502,7 @@ void vpKeyPoint::displayMatching( 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 nbImgSqrt = vpMath::round(std::sqrt((double)nbImg)); //(int) std::floor(std::sqrt((double) nbImg) + 0.5); int nbWidth = nbImgSqrt; int nbHeight = nbImgSqrt; @@ -1668,9 +1513,8 @@ void vpKeyPoint::displayMatching( std::map mapOfImageIdIndex; int cpt = 0; unsigned int maxW = ICurrent.getWidth(), maxH = ICurrent.getHeight(); - for (std::map >::const_iterator - it = m_mapOfImages.begin(); - it != m_mapOfImages.end(); ++it, cpt++) { + for (std::map >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); + ++it, cpt++) { mapOfImageIdIndex[it->first] = cpt; if (maxW < it->second.getWidth()) { @@ -1687,19 +1531,15 @@ void vpKeyPoint::displayMatching( int medianI = nbHeight / 2; int medianJ = nbWidth / 2; int medianIndex = medianI * nbWidth + medianJ; - for (std::vector::const_iterator it = - m_trainKeyPoints.begin(); - it != m_trainKeyPoints.end(); ++it) { + for (std::vector::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]]; + 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; + current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]] + 1; } int indexI = current_class_id_index / nbWidth; @@ -1707,71 +1547,48 @@ void vpKeyPoint::displayMatching( 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); + 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::const_iterator it = - m_queryKeyPoints.begin(); - it != m_queryKeyPoints.end(); ++it) { + for (std::vector::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); + vpDisplay::displayCross(IMatching, (int)(it->pt.y + topLeftCorner.get_i()), + (int)(it->pt.x + topLeftCorner.get_j()), crossSize, vpColor::red); } - for (std::vector::const_iterator it = ransacInliers.begin(); - it != ransacInliers.end(); ++it) { + for (std::vector::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); + vpDisplay::displayCircle(IMatching, (int)(it->get_v() + topLeftCorner.get_i()), + (int)(it->get_u() + topLeftCorner.get_j()), 4, vpColor::green); } - for (std::vector::const_iterator it = - m_ransacOutliers.begin(); - it != m_ransacOutliers.end(); ++it) { + for (std::vector::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); + vpDisplay::displayCircle(IMatching, (int)(it->get_i() + topLeftCorner.get_i()), + (int)(it->get_j() + topLeftCorner.get_j()), 4, vpColor::red); } - for (std::vector::const_iterator it = - m_filteredMatches.begin(); - it != m_filteredMatches.end(); ++it) { + for (std::vector::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]]; + 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; + 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); + 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); + vpDisplay::displayLine(IMatching, start, end, vpColor::green, lineThickness); } } } @@ -1786,9 +1603,7 @@ void vpKeyPoint::displayMatching( train points, when a keypoint cannot be extracted, we need to remove the corresponding 3D point. */ -void vpKeyPoint::extract(const vpImage &I, - std::vector &keyPoints, - cv::Mat &descriptors, +void vpKeyPoint::extract(const vpImage &I, std::vector &keyPoints, cv::Mat &descriptors, std::vector *trainPoints) { double elapsedTime; @@ -1805,9 +1620,7 @@ void vpKeyPoint::extract(const vpImage &I, 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 &keyPoints, - cv::Mat &descriptors, +void vpKeyPoint::extract(const cv::Mat &matImg, std::vector &keyPoints, cv::Mat &descriptors, std::vector *trainPoints) { double elapsedTime; @@ -1824,10 +1637,8 @@ void vpKeyPoint::extract(const cv::Mat &matImg, 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 &I, - std::vector &keyPoints, - cv::Mat &descriptors, double &elapsedTime, - std::vector *trainPoints) +void vpKeyPoint::extract(const vpImage &I, std::vector &keyPoints, cv::Mat &descriptors, + double &elapsedTime, std::vector *trainPoints) { cv::Mat matImg; vpImageConvert::convert(I, matImg, false); @@ -1844,17 +1655,13 @@ void vpKeyPoint::extract(const vpImage &I, 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 &keyPoints, - cv::Mat &descriptors, double &elapsedTime, - std::vector *trainPoints) +void vpKeyPoint::extract(const cv::Mat &matImg, std::vector &keyPoints, cv::Mat &descriptors, + double &elapsedTime, std::vector *trainPoints) { double t = vpTime::measureTimeMs(); bool first = true; - for (std::map >::const_iterator itd = - m_extractors.begin(); + for (std::map >::const_iterator itd = m_extractors.begin(); itd != m_extractors.end(); ++itd) { if (first) { first = false; @@ -1873,20 +1680,15 @@ void vpKeyPoint::extract(const cv::Mat &matImg, // keypoint as the value std::map mapOfKeypointHashes; size_t cpt = 0; - for (std::vector::const_iterator - it = keyPoints_tmp.begin(); - it != keyPoints_tmp.end(); ++it, cpt++) { + for (std::vector::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end(); + ++it, cpt++) { mapOfKeypointHashes[myKeypointHash(*it)] = cpt; } std::vector trainPoints_tmp; - for (std::vector::const_iterator it = - keyPoints.begin(); - it != keyPoints.end(); ++it) { - if (mapOfKeypointHashes.find(myKeypointHash(*it)) != - mapOfKeypointHashes.end()) { - trainPoints_tmp.push_back( - (*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]); + for (std::vector::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) { + if (mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) { + trainPoints_tmp.push_back((*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]); } } @@ -1912,26 +1714,21 @@ void vpKeyPoint::extract(const cv::Mat &matImg, // keypoint as the value std::map mapOfKeypointHashes; size_t cpt = 0; - for (std::vector::const_iterator - it = keyPoints_tmp.begin(); - it != keyPoints_tmp.end(); ++it, cpt++) { + for (std::vector::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end(); + ++it, cpt++) { mapOfKeypointHashes[myKeypointHash(*it)] = cpt; } std::vector trainPoints_tmp; cv::Mat descriptors_tmp; - for (std::vector::const_iterator it = keyPoints.begin(); - it != keyPoints.end(); ++it) { - if (mapOfKeypointHashes.find(myKeypointHash(*it)) != - mapOfKeypointHashes.end()) { + for (std::vector::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)]]); + trainPoints_tmp.push_back((*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]); } if (!descriptors.empty()) { - descriptors_tmp.push_back(descriptors.row( - (int)mapOfKeypointHashes[myKeypointHash(*it)])); + descriptors_tmp.push_back(descriptors.row((int)mapOfKeypointHashes[myKeypointHash(*it)])); } } } @@ -1992,8 +1789,7 @@ void vpKeyPoint::filterMatches() mean /= m_queryDescriptors.rows; } - double sq_sum = std::inner_product( - distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0); + 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; @@ -2001,27 +1797,20 @@ void vpKeyPoint::filterMatches() 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 = 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 (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]); + trainPts.push_back(m_trainPoints[(size_t)m_knnMatches[i][0].trainIdx]); } - queryKpts.push_back( - m_queryKeyPoints[(size_t)m_knnMatches[i][0].queryIdx]); + queryKpts.push_back(m_queryKeyPoints[(size_t)m_knnMatches[i][0].queryIdx]); } } } @@ -2046,22 +1835,20 @@ void vpKeyPoint::filterMatches() } mean /= m_queryDescriptors.rows; - double sq_sum = std::inner_product( - distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0); + 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; + // 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)); + 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]); @@ -2080,17 +1867,14 @@ void vpKeyPoint::filterMatches() std::map mapOfTrainIdx; // Count the number of query points matched to the same train point - for (std::vector::const_iterator it = m.begin(); - it != m.end(); ++it) { + for (std::vector::const_iterator it = m.begin(); it != m.end(); ++it) { mapOfTrainIdx[it->trainIdx]++; } // Keep matches with only one correspondence - for (std::vector::const_iterator it = m.begin(); - it != m.end(); ++it) { + for (std::vector::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)); + 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]); @@ -2139,10 +1923,7 @@ void vpKeyPoint::getObjectPoints(std::vector &objectPoints) const \param keyPoints : List of query keypoints (or keypoints detected in the current image). */ -void vpKeyPoint::getQueryKeyPoints(std::vector &keyPoints) const -{ - keyPoints = m_queryFilteredKeyPoints; -} +void vpKeyPoint::getQueryKeyPoints(std::vector &keyPoints) const { keyPoints = m_queryFilteredKeyPoints; } /*! Get the query keypoints list in ViSP type. @@ -2150,30 +1931,21 @@ void vpKeyPoint::getQueryKeyPoints(std::vector &keyPoints) const \param keyPoints : List of query keypoints (or keypoints detected in the current image). */ -void vpKeyPoint::getQueryKeyPoints(std::vector &keyPoints) const -{ - keyPoints = currentImagePointsList; -} +void vpKeyPoint::getQueryKeyPoints(std::vector &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 &keyPoints) const -{ - keyPoints = m_trainKeyPoints; -} +void vpKeyPoint::getTrainKeyPoints(std::vector &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 &keyPoints) const -{ - keyPoints = referenceImagePointsList; -} +void vpKeyPoint::getTrainKeyPoints(std::vector &keyPoints) const { keyPoints = referenceImagePointsList; } /*! Get the train points (the 3D coordinates in the object frame) list in @@ -2181,10 +1953,7 @@ void vpKeyPoint::getTrainKeyPoints(std::vector &keyPoints) const \param points : List of train points (or reference points). */ -void vpKeyPoint::getTrainPoints(std::vector &points) const -{ - points = m_trainPoints; -} +void vpKeyPoint::getTrainPoints(std::vector &points) const { points = m_trainPoints; } /*! Get the train points (the 3D coordinates in the object frame) list in ViSP @@ -2192,10 +1961,7 @@ void vpKeyPoint::getTrainPoints(std::vector &points) const \param points : List of train points (or reference points). */ -void vpKeyPoint::getTrainPoints(std::vector &points) const -{ - points = m_trainVpPoints; -} +void vpKeyPoint::getTrainPoints(std::vector &points) const { points = m_trainVpPoints; } /*! Initialize method for RANSAC parameters and for detectors, extractors and @@ -2204,20 +1970,16 @@ void vpKeyPoint::getTrainPoints(std::vector &points) const 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) +#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; + std::cerr << "Cannot init module non free, SIFT or SURF cannot be used." << std::endl; } #endif // Use k-nearest neighbors (knn) to retrieve the two best matches for a // keypoint So this is useful only for ratioDistanceThreshold method - if (m_filterType == ratioDistanceThreshold || - m_filterType == stdAndRatioDistanceThreshold) { + if (m_filterType == ratioDistanceThreshold || m_filterType == stdAndRatioDistanceThreshold) { m_useKnn = true; } @@ -2239,8 +2001,7 @@ void vpKeyPoint::initDetector(const std::string &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 << "."; + << " or it is not available in OpenCV version: " << std::hex << VISP_HAVE_OPENCV_VERSION << "."; throw vpException(vpException::fatalError, ss_msg.str()); } #else @@ -2255,171 +2016,138 @@ void vpKeyPoint::initDetector(const std::string &detectorName) if (detectorNameTmp == "SIFT") { #ifdef VISP_HAVE_OPENCV_XFEATURES2D - cv::Ptr siftDetector = - cv::xfeatures2d::SIFT::create(); + cv::Ptr siftDetector = cv::xfeatures2d::SIFT::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = siftDetector; } else { - std::cerr << "You should not use SIFT with Pyramid feature detection!" - << std::endl; - m_detectors[detectorName] = - cv::makePtr(siftDetector); + std::cerr << "You should not use SIFT with Pyramid feature detection!" << std::endl; + m_detectors[detectorName] = cv::makePtr(siftDetector); } #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the detector: SIFT. OpenCV version " - << std::hex << VISP_HAVE_OPENCV_VERSION + 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 surfDetector = - cv::xfeatures2d::SURF::create(); + cv::Ptr surfDetector = cv::xfeatures2d::SURF::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = surfDetector; } else { - std::cerr << "You should not use SURF with Pyramid feature detection!" - << std::endl; - m_detectors[detectorName] = - cv::makePtr(surfDetector); + std::cerr << "You should not use SURF with Pyramid feature detection!" << std::endl; + m_detectors[detectorName] = cv::makePtr(surfDetector); } #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the detector: SURF. OpenCV version " - << std::hex << VISP_HAVE_OPENCV_VERSION + 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 fastDetector = - cv::FastFeatureDetector::create(); + cv::Ptr fastDetector = cv::FastFeatureDetector::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = fastDetector; } else { - m_detectors[detectorName] = - cv::makePtr(fastDetector); + m_detectors[detectorName] = cv::makePtr(fastDetector); } } else if (detectorNameTmp == "MSER") { cv::Ptr fastDetector = cv::MSER::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = fastDetector; } else { - m_detectors[detectorName] = - cv::makePtr(fastDetector); + m_detectors[detectorName] = cv::makePtr(fastDetector); } } else if (detectorNameTmp == "ORB") { cv::Ptr orbDetector = cv::ORB::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = orbDetector; } else { - std::cerr << "You should not use ORB with Pyramid feature detection!" - << std::endl; - m_detectors[detectorName] = - cv::makePtr(orbDetector); + std::cerr << "You should not use ORB with Pyramid feature detection!" << std::endl; + m_detectors[detectorName] = cv::makePtr(orbDetector); } } else if (detectorNameTmp == "BRISK") { cv::Ptr briskDetector = cv::BRISK::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = briskDetector; } else { - std::cerr << "You should not use BRISK with Pyramid feature detection!" - << std::endl; - m_detectors[detectorName] = - cv::makePtr(briskDetector); + std::cerr << "You should not use BRISK with Pyramid feature detection!" << std::endl; + m_detectors[detectorName] = cv::makePtr(briskDetector); } } else if (detectorNameTmp == "KAZE") { cv::Ptr kazeDetector = cv::KAZE::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = kazeDetector; } else { - std::cerr << "You should not use KAZE with Pyramid feature detection!" - << std::endl; - m_detectors[detectorName] = - cv::makePtr(kazeDetector); + std::cerr << "You should not use KAZE with Pyramid feature detection!" << std::endl; + m_detectors[detectorName] = cv::makePtr(kazeDetector); } } else if (detectorNameTmp == "AKAZE") { cv::Ptr akazeDetector = cv::AKAZE::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = akazeDetector; } else { - std::cerr << "You should not use AKAZE with Pyramid feature detection!" - << std::endl; - m_detectors[detectorName] = - cv::makePtr(akazeDetector); + std::cerr << "You should not use AKAZE with Pyramid feature detection!" << std::endl; + m_detectors[detectorName] = cv::makePtr(akazeDetector); } } else if (detectorNameTmp == "GFTT") { cv::Ptr gfttDetector = cv::GFTTDetector::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = gfttDetector; } else { - m_detectors[detectorName] = - cv::makePtr(gfttDetector); + m_detectors[detectorName] = cv::makePtr(gfttDetector); } } else if (detectorNameTmp == "SimpleBlob") { - cv::Ptr simpleBlobDetector = - cv::SimpleBlobDetector::create(); + cv::Ptr simpleBlobDetector = cv::SimpleBlobDetector::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = simpleBlobDetector; } else { - m_detectors[detectorName] = - cv::makePtr(simpleBlobDetector); + m_detectors[detectorName] = cv::makePtr(simpleBlobDetector); } } else if (detectorNameTmp == "STAR") { #ifdef VISP_HAVE_OPENCV_XFEATURES2D - cv::Ptr starDetector = - cv::xfeatures2d::StarDetector::create(); + cv::Ptr starDetector = cv::xfeatures2d::StarDetector::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = starDetector; } else { - m_detectors[detectorName] = - cv::makePtr(starDetector); + m_detectors[detectorName] = cv::makePtr(starDetector); } #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the detector: STAR. OpenCV version " - << std::hex << VISP_HAVE_OPENCV_VERSION + 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 agastDetector = - cv::AgastFeatureDetector::create(); + cv::Ptr agastDetector = cv::AgastFeatureDetector::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = agastDetector; } else { - m_detectors[detectorName] = - cv::makePtr(agastDetector); + m_detectors[detectorName] = cv::makePtr(agastDetector); } } else if (detectorNameTmp == "MSD") { #if (VISP_HAVE_OPENCV_VERSION >= 0x030100) #if defined(VISP_HAVE_OPENCV_XFEATURES2D) - cv::Ptr msdDetector = - cv::xfeatures2d::MSDDetector::create(); + cv::Ptr msdDetector = cv::xfeatures2d::MSDDetector::create(); if (!usePyramid) { m_detectors[detectorNameTmp] = msdDetector; } else { - std::cerr << "You should not use MSD with Pyramid feature detection!" - << std::endl; - m_detectors[detectorName] = - cv::makePtr(msdDetector); + std::cerr << "You should not use MSD with Pyramid feature detection!" << std::endl; + m_detectors[detectorName] = cv::makePtr(msdDetector); } #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the detector: MSD. OpenCV version " - << std::hex << VISP_HAVE_OPENCV_VERSION + ss_msg << "Fail to initialize the detector: MSD. OpenCV version " << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module."; throw vpException(vpException::fatalError, ss_msg.str()); #endif #else std::stringstream ss_msg; - ss_msg << "Feature " << detectorName - << " is not available in OpenCV version: " << std::hex + ss_msg << "Feature " << detectorName << " is not available in OpenCV version: " << std::hex << VISP_HAVE_OPENCV_VERSION << " (require >= OpenCV 3.1)."; #endif } else { - std::cerr << "The detector:" << detectorNameTmp << " is not available." - << std::endl; + std::cerr << "The detector:" << detectorNameTmp << " is not available." << std::endl; } bool detectorInitialized = false; @@ -2432,8 +2160,7 @@ void vpKeyPoint::initDetector(const std::string &detectorName) 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 << "."; + << " or it is not available in OpenCV version: " << std::hex << VISP_HAVE_OPENCV_VERSION << "."; throw vpException(vpException::fatalError, ss_msg.str()); } @@ -2448,8 +2175,7 @@ void vpKeyPoint::initDetector(const std::string &detectorName) */ void vpKeyPoint::initDetectors(const std::vector &detectorNames) { - for (std::vector::const_iterator it = detectorNames.begin(); - it != detectorNames.end(); ++it) { + for (std::vector::const_iterator it = detectorNames.begin(); it != detectorNames.end(); ++it) { initDetector(*it); } } @@ -2462,28 +2188,24 @@ void vpKeyPoint::initDetectors(const std::vector &detectorNames) void vpKeyPoint::initExtractor(const std::string &extractorName) { #if (VISP_HAVE_OPENCV_VERSION < 0x030000) - m_extractors[extractorName] = - cv::DescriptorExtractor::create(extractorName); + 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 + 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); + 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 + 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 @@ -2496,20 +2218,17 @@ void vpKeyPoint::initExtractor(const std::string &extractorName) m_extractors[extractorName] = cv::xfeatures2d::FREAK::create(); #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the extractor: " << extractorName - << ". OpenCV version " << std::hex << VISP_HAVE_OPENCV_VERSION - << " was not build with xFeatures2d module."; + ss_msg << "Fail to initialize the extractor: " << extractorName << ". 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(); + m_extractors[extractorName] = cv::xfeatures2d::BriefDescriptorExtractor::create(); #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the extractor: " << extractorName - << ". OpenCV version " << std::hex << VISP_HAVE_OPENCV_VERSION - << " was not build with xFeatures2d module."; + ss_msg << "Fail to initialize the extractor: " << extractorName << ". 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") { @@ -2521,9 +2240,8 @@ void vpKeyPoint::initExtractor(const std::string &extractorName) m_extractors[extractorName] = cv::xfeatures2d::DAISY::create(); #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the extractor: " << extractorName - << ". OpenCV version " << std::hex << VISP_HAVE_OPENCV_VERSION - << " was not build with xFeatures2d module."; + ss_msg << "Fail to initialize the extractor: " << extractorName << ". 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") { @@ -2531,22 +2249,19 @@ void vpKeyPoint::initExtractor(const std::string &extractorName) m_extractors[extractorName] = cv::xfeatures2d::LATCH::create(); #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the extractor: " << extractorName - << ". OpenCV version " << std::hex << VISP_HAVE_OPENCV_VERSION - << " was not build with xFeatures2d module."; + ss_msg << "Fail to initialize the extractor: " << extractorName << ". 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); // Not possible currently, need a color image - throw vpException(vpException::badValue, - "Not possible currently as it needs a color image."); + throw vpException(vpException::badValue, "Not possible currently as it needs a color image."); #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the extractor: " << extractorName - << ". OpenCV version " << std::hex << VISP_HAVE_OPENCV_VERSION - << " was not build with xFeatures2d module."; + ss_msg << "Fail to initialize the extractor: " << extractorName << ". 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 == "VGG") { @@ -2555,16 +2270,14 @@ void vpKeyPoint::initExtractor(const std::string &extractorName) m_extractors[extractorName] = cv::xfeatures2d::VGG::create(); #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the extractor: " << extractorName - << ". OpenCV version " << std::hex << VISP_HAVE_OPENCV_VERSION - << " was not build with xFeatures2d module."; + ss_msg << "Fail to initialize the extractor: " << extractorName << ". OpenCV version " << std::hex + << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module."; throw vpException(vpException::fatalError, ss_msg.str()); #endif #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the extractor: " << extractorName - << ". OpenCV version " << std::hex << VISP_HAVE_OPENCV_VERSION - << " but requires at least OpenCV 3.2."; + ss_msg << "Fail to initialize the extractor: " << extractorName << ". OpenCV version " << std::hex + << VISP_HAVE_OPENCV_VERSION << " but requires at least OpenCV 3.2."; throw vpException(vpException::fatalError, ss_msg.str()); #endif } else if (extractorName == "BoostDesc") { @@ -2573,34 +2286,29 @@ void vpKeyPoint::initExtractor(const std::string &extractorName) m_extractors[extractorName] = cv::xfeatures2d::BoostDesc::create(); #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the extractor: " << extractorName - << ". OpenCV version " << std::hex << VISP_HAVE_OPENCV_VERSION - << " was not build with xFeatures2d module."; + ss_msg << "Fail to initialize the extractor: " << extractorName << ". OpenCV version " << std::hex + << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module."; throw vpException(vpException::fatalError, ss_msg.str()); #endif #else std::stringstream ss_msg; - ss_msg << "Fail to initialize the extractor: " << extractorName - << ". OpenCV version " << std::hex << VISP_HAVE_OPENCV_VERSION - << " but requires at least OpenCV 3.2."; + ss_msg << "Fail to initialize the extractor: " << extractorName << ". OpenCV version " << std::hex + << VISP_HAVE_OPENCV_VERSION << " but requires at least OpenCV 3.2."; throw vpException(vpException::fatalError, ss_msg.str()); #endif } else { - std::cerr << "The extractor:" << extractorName << " is not available." - << std::endl; + 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 << "."; + << " 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 (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); @@ -2614,27 +2322,22 @@ void vpKeyPoint::initExtractor(const std::string &extractorName) \param extractorNames : List of extractor names. */ -void vpKeyPoint::initExtractors( - const std::vector &extractorNames) +void vpKeyPoint::initExtractors(const std::vector &extractorNames) { - for (std::vector::const_iterator it = extractorNames.begin(); - it != extractorNames.end(); ++it) { + for (std::vector::const_iterator it = extractorNames.begin(); it != extractorNames.end(); ++it) { initExtractor(*it); } int descriptorType = CV_32F; bool firstIteration = true; - for (std::map >::const_iterator it = - m_extractors.begin(); + for (std::map >::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 !"); + throw vpException(vpException::fatalError, "All the descriptors must have the same type !"); } } } @@ -2650,8 +2353,7 @@ void vpKeyPoint::initFeatureNames() m_mapOfDetectorNames[DETECTOR_BRISK] = "BRISK"; m_mapOfDetectorNames[DETECTOR_GFTT] = "GFTT"; m_mapOfDetectorNames[DETECTOR_SimpleBlob] = "SimpleBlob"; -#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || \ - (defined(VISP_HAVE_OPENCV_XFEATURES2D)) +#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || (defined(VISP_HAVE_OPENCV_XFEATURES2D)) m_mapOfDetectorNames[DETECTOR_STAR] = "STAR"; #endif #if defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D) @@ -2663,8 +2365,7 @@ void vpKeyPoint::initFeatureNames() m_mapOfDetectorNames[DETECTOR_AKAZE] = "AKAZE"; m_mapOfDetectorNames[DETECTOR_AGAST] = "AGAST"; #endif -#if (VISP_HAVE_OPENCV_VERSION >= 0x030100) && \ - defined(VISP_HAVE_OPENCV_XFEATURES2D) +#if (VISP_HAVE_OPENCV_VERSION >= 0x030100) && defined(VISP_HAVE_OPENCV_XFEATURES2D) m_mapOfDetectorNames[DETECTOR_MSD] = "MSD"; #endif #endif @@ -2672,8 +2373,7 @@ void vpKeyPoint::initFeatureNames() #if (VISP_HAVE_OPENCV_VERSION >= 0x020403) m_mapOfDescriptorNames[DESCRIPTOR_ORB] = "ORB"; m_mapOfDescriptorNames[DESCRIPTOR_BRISK] = "BRISK"; -#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || \ - (defined(VISP_HAVE_OPENCV_XFEATURES2D)) +#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || (defined(VISP_HAVE_OPENCV_XFEATURES2D)) m_mapOfDescriptorNames[DESCRIPTOR_FREAK] = "FREAK"; m_mapOfDescriptorNames[DESCRIPTOR_BRIEF] = "BRIEF"; #endif @@ -2689,8 +2389,7 @@ void vpKeyPoint::initFeatureNames() m_mapOfDescriptorNames[DESCRIPTOR_LATCH] = "LATCH"; #endif #endif -#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && \ - defined(VISP_HAVE_OPENCV_XFEATURES2D) +#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && defined(VISP_HAVE_OPENCV_XFEATURES2D) m_mapOfDescriptorNames[DESCRIPTOR_VGG] = "VGG"; m_mapOfDescriptorNames[DESCRIPTOR_BoostDesc] = "BoostDesc"; #endif @@ -2706,17 +2405,14 @@ void vpKeyPoint::initMatcher(const std::string &matcherName) { int descriptorType = CV_32F; bool firstIteration = true; - for (std::map >::const_iterator it = - m_extractors.begin(); + for (std::map >::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 !"); + throw vpException(vpException::fatalError, "All the descriptors must have the same type !"); } } } @@ -2731,27 +2427,22 @@ void vpKeyPoint::initMatcher(const std::string &matcherName) if (descriptorType == CV_8U) { #if (VISP_HAVE_OPENCV_VERSION >= 0x030000) - m_matcher = cv::makePtr( - cv::makePtr(12, 20, 2)); + m_matcher = cv::makePtr(cv::makePtr(12, 20, 2)); #else - m_matcher = - new cv::FlannBasedMatcher(new cv::flann::LshIndexParams(12, 20, 2)); + 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::makePtr()); + m_matcher = cv::makePtr(cv::makePtr()); #else - m_matcher = - new cv::FlannBasedMatcher(new cv::flann::KDTreeIndexParams()); + 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 (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000) if (m_matcher != NULL && !m_useKnn && matcherName == "BruteForce") { m_matcher->set("crossCheck", m_useBruteForceCrossCheck); } @@ -2760,8 +2451,7 @@ void vpKeyPoint::initMatcher(const std::string &matcherName) 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 << "."; + << " or it is not available in OpenCV version: " << std::hex << VISP_HAVE_OPENCV_VERSION << "."; throw vpException(vpException::fatalError, ss_msg.str()); } } @@ -2774,8 +2464,7 @@ void vpKeyPoint::initMatcher(const std::string &matcherName) \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 &IRef, - const vpImage &ICurrent, +void vpKeyPoint::insertImageMatching(const vpImage &IRef, const vpImage &ICurrent, vpImage &IMatching) { vpImagePoint topLeftCorner(0, 0); @@ -2791,8 +2480,7 @@ void vpKeyPoint::insertImageMatching(const vpImage &IRef, \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 &ICurrent, - vpImage &IMatching) +void vpKeyPoint::insertImageMatching(const vpImage &ICurrent, vpImage &IMatching) { // Nb images in the training database + the current image we want to detect // the object @@ -2808,8 +2496,7 @@ void vpKeyPoint::insertImageMatching(const vpImage &ICurrent, 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 nbImgSqrt = vpMath::round(std::sqrt((double)nbImg)); //(int) std::floor(std::sqrt((double) nbImg) + 0.5); int nbWidth = nbImgSqrt; int nbHeight = nbImgSqrt; @@ -2818,9 +2505,8 @@ void vpKeyPoint::insertImageMatching(const vpImage &ICurrent, } unsigned int maxW = ICurrent.getWidth(), maxH = ICurrent.getHeight(); - for (std::map >::const_iterator it = - m_mapOfImages.begin(); - it != m_mapOfImages.end(); ++it) { + for (std::map >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); + ++it) { if (maxW < it->second.getWidth()) { maxW = it->second.getWidth(); } @@ -2837,9 +2523,8 @@ void vpKeyPoint::insertImageMatching(const vpImage &ICurrent, int medianIndex = medianI * nbWidth + medianJ; int cpt = 0; - for (std::map >::const_iterator - it = m_mapOfImages.begin(); - it != m_mapOfImages.end(); ++it, cpt++) { + for (std::map >::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 @@ -2920,8 +2605,7 @@ void vpKeyPoint::loadConfigFile(const std::string &configFile) m_ransacThreshold = xmlp.getRansacThreshold(); m_ransacConsensusPercentage = xmlp.getRansacConsensusPercentage(); - if (m_filterType == ratioDistanceThreshold || - m_filterType == stdAndRatioDistanceThreshold) { + if (m_filterType == ratioDistanceThreshold || m_filterType == stdAndRatioDistanceThreshold) { m_useKnn = true; } else { m_useKnn = false; @@ -2929,8 +2613,7 @@ void vpKeyPoint::loadConfigFile(const std::string &configFile) init(); } catch (...) { - throw vpException(vpException::ioError, "Can't open XML file \"%s\"\n ", - configFile.c_str()); + throw vpException(vpException::ioError, "Can't open XML file \"%s\"\n ", configFile.c_str()); } } #endif @@ -2943,8 +2626,7 @@ void vpKeyPoint::loadConfigFile(const std::string &configFile) 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) +void vpKeyPoint::loadLearningData(const std::string &filename, const bool binaryMode, const bool append) { int startClassId = 0; int startImageId = 0; @@ -2955,17 +2637,15 @@ void vpKeyPoint::loadLearningData(const std::string &filename, m_mapOfImages.clear(); } else { // In append case, find the max index of keypoint class Id - for (std::map::const_iterator it = m_mapOfImageId.begin(); - it != m_mapOfImageId.end(); ++it) { + for (std::map::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 >::const_iterator it = - m_mapOfImages.begin(); - it != m_mapOfImages.end(); ++it) { + for (std::map >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); + ++it) { if (startImageId < it->first) { startImageId = it->first; } @@ -3060,15 +2740,13 @@ void vpKeyPoint::loadLearningData(const std::string &filename, readBinaryIntLE(file, octave); readBinaryIntLE(file, class_id); readBinaryIntLE(file, image_id); - cv::KeyPoint keyPoint(cv::Point2f(u, v), size, angle, response, octave, - (class_id + startClassId)); + 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[m_trainKeyPoints.back().class_id] = - image_id + startImageId; + m_mapOfImageId[m_trainKeyPoints.back().class_id] = image_id + startImageId; #endif } @@ -3138,8 +2816,7 @@ void vpKeyPoint::loadLearningData(const std::string &filename, if (!append || m_trainDescriptors.empty()) { trainDescriptorsTmp.copyTo(m_trainDescriptors); } else { - cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, - m_trainDescriptors); + cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors); } file.close(); @@ -3173,22 +2850,18 @@ void vpKeyPoint::loadLearningData(const std::string &filename, cv::Mat trainDescriptorsTmp; - for (first_level_node = root_element->children; first_level_node; - first_level_node = first_level_node->next) { + 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") { + 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) { + 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"); + 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); @@ -3210,31 +2883,25 @@ void vpKeyPoint::loadLearningData(const std::string &filename, #endif } } - } else if (first_level_node->type == XML_ELEMENT_NODE && - name == "DescriptorsInfo") { + } 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; + 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); + nRows = std::atoi((char *)descriptors_info_node->children->content); } else if (name == "ncols") { - nCols = - std::atoi((char *)descriptors_info_node->children->content); + nCols = std::atoi((char *)descriptors_info_node->children->content); } else if (name == "type") { - descriptorType = - std::atoi((char *)descriptors_info_node->children->content); + 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") { + } 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; @@ -3242,8 +2909,7 @@ void vpKeyPoint::loadLearningData(const std::string &filename, std::stringstream ss; - for (point_node = first_level_node->children; point_node; - point_node = point_node->next) { + 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); @@ -3253,29 +2919,24 @@ void vpKeyPoint::loadLearningData(const std::string &filename, } 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); + size = std::strtod((char *)point_node->children->content, &pEnd); } else if (name == "angle") { - angle = - std::strtod((char *)point_node->children->content, &pEnd); + angle = std::strtod((char *)point_node->children->content, &pEnd); } else if (name == "response") { - response = - std::strtod((char *)point_node->children->content, &pEnd); + 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)); + 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; + m_mapOfImageId[m_trainKeyPoints.back().class_id] = image_id + startImageId; #endif } } else if (name == "oX") { @@ -3284,20 +2945,17 @@ void vpKeyPoint::loadLearningData(const std::string &filename, 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)); + 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; + 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); + std::string parseStr((char *)descriptor_value_node->children->content); ss.clear(); ss.str(parseStr); @@ -3307,8 +2965,7 @@ void vpKeyPoint::loadLearningData(const std::string &filename, // Parse the numeric value [0 ; 255] to an int int parseValue; ss >> parseValue; - trainDescriptorsTmp.at(i, j) = - (unsigned char)parseValue; + trainDescriptorsTmp.at(i, j) = (unsigned char)parseValue; } break; case CV_8S: @@ -3343,8 +3000,7 @@ void vpKeyPoint::loadLearningData(const std::string &filename, break; } } else { - std::cerr << "Error when converting:" << ss.str() - << std::endl; + std::cerr << "Error when converting:" << ss.str() << std::endl; } j++; @@ -3360,8 +3016,7 @@ void vpKeyPoint::loadLearningData(const std::string &filename, if (!append || m_trainDescriptors.empty()) { trainDescriptorsTmp.copyTo(m_trainDescriptors); } else { - cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, - m_trainDescriptors); + cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors); } /*free the document */ @@ -3400,8 +3055,7 @@ void vpKeyPoint::loadLearningData(const std::string &filename, \param matches : Output list of matches. \param elapsedTime : Elapsed time. */ -void vpKeyPoint::match(const cv::Mat &trainDescriptors, - const cv::Mat &queryDescriptors, +void vpKeyPoint::match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors, std::vector &matches, double &elapsedTime) { double t = vpTime::measureTimeMs(); @@ -3414,30 +3068,24 @@ void vpKeyPoint::match(const cv::Mat &trainDescriptors, // Match train descriptors to query descriptors cv::Ptr matcherTmp = m_matcher->clone(true); - matcherTmp->knnMatch(trainDescriptors, queryDescriptors, knnMatchesTmp, - 2); + matcherTmp->knnMatch(trainDescriptors, queryDescriptors, knnMatchesTmp, 2); - for (std::vector >::const_iterator it1 = - knnMatchesTmp.begin(); + for (std::vector >::const_iterator it1 = knnMatchesTmp.begin(); it1 != knnMatchesTmp.end(); ++it1) { std::vector tmp; - for (std::vector::const_iterator it2 = it1->begin(); - it2 != it1->end(); ++it2) { - tmp.push_back( - cv::DMatch(it2->trainIdx, it2->queryIdx, it2->distance)); + for (std::vector::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); + 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); + std::transform(m_knnMatches.begin(), m_knnMatches.end(), matches.begin(), knnToDMatch); } } else { matches.clear(); @@ -3448,10 +3096,8 @@ void vpKeyPoint::match(const cv::Mat &trainDescriptors, cv::Ptr matcherTmp = m_matcher->clone(true); matcherTmp->match(trainDescriptors, queryDescriptors, matchesTmp); - for (std::vector::const_iterator it = matchesTmp.begin(); - it != matchesTmp.end(); ++it) { - matches.push_back( - cv::DMatch(it->trainIdx, it->queryIdx, it->distance)); + for (std::vector::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 @@ -3468,10 +3114,7 @@ void vpKeyPoint::match(const cv::Mat &trainDescriptors, \param I : Input current image. \return The number of matched keypoints. */ -unsigned int vpKeyPoint::matchPoint(const vpImage &I) -{ - return matchPoint(I, vpRect()); -} +unsigned int vpKeyPoint::matchPoint(const vpImage &I) { return matchPoint(I, vpRect()); } /*! Match keypoints detected in a region of interest of the image with those @@ -3483,9 +3126,7 @@ unsigned int vpKeyPoint::matchPoint(const vpImage &I) \param width : Width of the region of interest \return The number of matched keypoints */ -unsigned int vpKeyPoint::matchPoint(const vpImage &I, - const vpImagePoint &iP, - const unsigned int height, +unsigned int vpKeyPoint::matchPoint(const vpImage &I, const vpImagePoint &iP, const unsigned int height, const unsigned int width) { return matchPoint(I, vpRect(iP, width, height)); @@ -3499,8 +3140,7 @@ unsigned int vpKeyPoint::matchPoint(const vpImage &I, \param rectangle : Rectangle of the region of interest \return The number of matched keypoints */ -unsigned int vpKeyPoint::matchPoint(const vpImage &I, - const vpRect &rectangle) +unsigned int vpKeyPoint::matchPoint(const vpImage &I, const vpRect &rectangle) { if (m_trainDescriptors.empty()) { std::cerr << "Reference is empty." << std::endl; @@ -3521,16 +3161,14 @@ unsigned int vpKeyPoint::matchPoint(const vpImage &I, // Flatten the different train lists m_queryKeyPoints.clear(); - for (std::vector >::const_iterator it = - listOfQueryKeyPoints.begin(); + for (std::vector >::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::const_iterator it = - listOfQueryDescriptors.begin(); - it != listOfQueryDescriptors.end(); ++it) { + for (std::vector::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); + ++it) { if (first) { first = false; it->copyTo(m_queryDescriptors); @@ -3556,13 +3194,9 @@ unsigned int vpKeyPoint::matchPoint(const vpImage &I, // Add only query keypoints matched with a train keypoints m_queryFilteredKeyPoints.clear(); m_filteredMatches.clear(); - for (std::vector::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]); + for (std::vector::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; @@ -3574,8 +3208,7 @@ unsigned int vpKeyPoint::matchPoint(const vpImage &I, // 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::const_iterator it = m_matches.begin(); - it != m_matches.end(); ++it) { + for (std::vector::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]); } @@ -3583,8 +3216,7 @@ unsigned int vpKeyPoint::matchPoint(const vpImage &I, } // Convert OpenCV type to ViSP type for compatibility - vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, - currentImagePointsList); + vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, currentImagePointsList); vpConvert::convertFromOpenCV(m_filteredMatches, matchedReferencePoints); return static_cast(m_filteredMatches.size()); @@ -3603,11 +3235,8 @@ unsigned int vpKeyPoint::matchPoint(const vpImage &I, Interest) to consider \return True if the matching and the pose estimation are OK, false otherwise */ -bool vpKeyPoint::matchPoint(const vpImage &I, - const vpCameraParameters &cam, - vpHomogeneousMatrix &cMo, - bool (*func)(vpHomogeneousMatrix *), - const vpRect &rectangle) +bool vpKeyPoint::matchPoint(const vpImage &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, + bool (*func)(vpHomogeneousMatrix *), const vpRect &rectangle) { double error, elapsedTime; return matchPoint(I, cam, cMo, error, elapsedTime, func, rectangle); @@ -3629,11 +3258,8 @@ bool vpKeyPoint::matchPoint(const vpImage &I, Interest) to consider \return True if the matching and the pose estimation are OK, false otherwise */ -bool vpKeyPoint::matchPoint(const vpImage &I, - const vpCameraParameters &cam, - vpHomogeneousMatrix &cMo, double &error, - double &elapsedTime, - bool (*func)(vpHomogeneousMatrix *), +bool vpKeyPoint::matchPoint(const vpImage &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, + double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *), const vpRect &rectangle) { // Check if we have training descriptors @@ -3656,16 +3282,14 @@ bool vpKeyPoint::matchPoint(const vpImage &I, // Flatten the different train lists m_queryKeyPoints.clear(); - for (std::vector >::const_iterator it = - listOfQueryKeyPoints.begin(); + for (std::vector >::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::const_iterator it = - listOfQueryDescriptors.begin(); - it != listOfQueryDescriptors.end(); ++it) { + for (std::vector::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); + ++it) { if (first) { first = false; it->copyTo(m_queryDescriptors); @@ -3693,13 +3317,9 @@ bool vpKeyPoint::matchPoint(const vpImage &I, // Add only query keypoints matched with a train keypoints m_queryFilteredKeyPoints.clear(); m_filteredMatches.clear(); - for (std::vector::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]); + for (std::vector::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; @@ -3711,8 +3331,7 @@ bool vpKeyPoint::matchPoint(const vpImage &I, // 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::const_iterator it = m_matches.begin(); - it != m_matches.end(); ++it) { + for (std::vector::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]); } @@ -3720,8 +3339,7 @@ bool vpKeyPoint::matchPoint(const vpImage &I, } // Convert OpenCV type to ViSP type for compatibility - vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, - currentImagePointsList); + vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, currentImagePointsList); vpConvert::convertFromOpenCV(m_filteredMatches, matchedReferencePoints); // error = std::numeric_limits::max(); // create an error under @@ -3735,14 +3353,12 @@ bool vpKeyPoint::matchPoint(const vpImage &I, size_t cpt = 0; // Create a list of vpPoint with 2D coordinates (current keypoint // location) + 3D coordinates (world/object coordinates) - for (std::vector::const_iterator - it = m_objectFilteredPoints.begin(); + for (std::vector::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); + 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); @@ -3755,35 +3371,27 @@ bool vpKeyPoint::matchPoint(const vpImage &I, std::vector inliers; std::vector inlierIndex; - bool res = computePose(objectVpPoints, cMo, inliers, inlierIndex, - m_poseTime, func); + bool res = computePose(objectVpPoints, cMo, inliers, inlierIndex, m_poseTime, func); std::map mapOfInlierIndex; m_matchRansacKeyPointsToPoints.clear(); - for (std::vector::const_iterator it = inlierIndex.begin(); - it != inlierIndex.end(); ++it) { - m_matchRansacKeyPointsToPoints.push_back( - std::pair( - m_queryFilteredKeyPoints[(size_t)(*it)], - m_objectFilteredPoints[(size_t)(*it)])); + for (std::vector::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) { + m_matchRansacKeyPointsToPoints.push_back(std::pair( + 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)); + m_ransacOutliers.push_back(vpImagePoint(m_queryFilteredKeyPoints[i].pt.y, m_queryFilteredKeyPoints[i].pt.x)); } } - error = - computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo); + error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo); m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size()); - std::transform(m_matchRansacKeyPointsToPoints.begin(), - m_matchRansacKeyPointsToPoints.end(), + std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(), matchRansacToVpImage); elapsedTime += m_poseTime; @@ -3793,35 +3401,27 @@ bool vpKeyPoint::matchPoint(const vpImage &I, std::vector imageFilteredPoints; cv::KeyPoint::convert(m_queryFilteredKeyPoints, imageFilteredPoints); std::vector inlierIndex; - bool res = computePose(imageFilteredPoints, m_objectFilteredPoints, cam, - cMo, inlierIndex, m_poseTime); + bool res = computePose(imageFilteredPoints, m_objectFilteredPoints, cam, cMo, inlierIndex, m_poseTime); std::map mapOfInlierIndex; m_matchRansacKeyPointsToPoints.clear(); - for (std::vector::const_iterator it = inlierIndex.begin(); - it != inlierIndex.end(); ++it) { - m_matchRansacKeyPointsToPoints.push_back( - std::pair( - m_queryFilteredKeyPoints[(size_t)(*it)], - m_objectFilteredPoints[(size_t)(*it)])); + for (std::vector::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) { + m_matchRansacKeyPointsToPoints.push_back(std::pair( + 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)); + m_ransacOutliers.push_back(vpImagePoint(m_queryFilteredKeyPoints[i].pt.y, m_queryFilteredKeyPoints[i].pt.x)); } } - error = - computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo); + error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo); m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size()); - std::transform(m_matchRansacKeyPointsToPoints.begin(), - m_matchRansacKeyPointsToPoints.end(), + std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(), matchRansacToVpImage); elapsedTime += m_poseTime; @@ -3848,12 +3448,10 @@ bool vpKeyPoint::matchPoint(const vpImage &I, \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 &I, vpRect &boundingBox, - vpImagePoint ¢erOfGravity, const bool isPlanarObject, - std::vector *imPts1, std::vector *imPts2, - double *meanDescriptorDistance, double *detection_score, - const vpRect &rectangle) +bool vpKeyPoint::matchPointAndDetect(const vpImage &I, vpRect &boundingBox, + vpImagePoint ¢erOfGravity, const bool isPlanarObject, + std::vector *imPts1, std::vector *imPts2, + double *meanDescriptorDistance, double *detection_score, const vpRect &rectangle) { if (imPts1 != NULL && imPts2 != NULL) { imPts1->clear(); @@ -3863,8 +3461,7 @@ bool vpKeyPoint::matchPointAndDetect( matchPoint(I, rectangle); double meanDescriptorDistanceTmp = 0.0; - for (std::vector::const_iterator it = m_filteredMatches.begin(); - it != m_filteredMatches.end(); ++it) { + for (std::vector::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) { meanDescriptorDistanceTmp += (double)it->distance; } @@ -3885,20 +3482,16 @@ bool vpKeyPoint::matchPointAndDetect( std::vector 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); + 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 inliers; if (isPlanarObject) { #if (VISP_HAVE_OPENCV_VERSION < 0x030000) - cv::Mat homographyMatrix = - cv::findHomography(points1, points2, CV_RANSAC); + cv::Mat homographyMatrix = cv::findHomography(points1, points2, CV_RANSAC); #else - cv::Mat homographyMatrix = - cv::findHomography(points1, points2, cv::RANSAC); + cv::Mat homographyMatrix = cv::findHomography(points1, points2, cv::RANSAC); #endif for (size_t i = 0; i < m_filteredMatches.size(); i++) { @@ -3909,46 +3502,35 @@ bool vpKeyPoint::matchPointAndDetect( realPoint.at(2, 0) = 1.f; cv::Mat reprojectedPoint = homographyMatrix * realPoint; - double err_x = (reprojectedPoint.at(0, 0) / - reprojectedPoint.at(2, 0)) - - points2[i].x; - double err_y = (reprojectedPoint.at(1, 0) / - reprojectedPoint.at(2, 0)) - - points2[i].y; + double err_x = (reprojectedPoint.at(0, 0) / reprojectedPoint.at(2, 0)) - points2[i].x; + double err_y = (reprojectedPoint.at(1, 0) / reprojectedPoint.at(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)); + 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)); + 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)); + 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); + 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((int)i, 0)) { - inliers.push_back( - vpImagePoint((double)points2[i].y, (double)points2[i].x)); + 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)); + 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)); + imPts2->push_back(vpImagePoint((double)points2[i].y, (double)points2[i].x)); } } } @@ -3962,8 +3544,7 @@ bool vpKeyPoint::matchPointAndDetect( // Compute the center of gravity double meanU = 0.0, meanV = 0.0; - for (std::vector::const_iterator it = inliers.begin(); - it != inliers.end(); ++it) { + for (std::vector::const_iterator it = inliers.begin(); it != inliers.end(); ++it) { meanU += it->get_u(); meanV += it->get_v(); } @@ -4006,24 +3587,19 @@ bool vpKeyPoint::matchPointAndDetect( consider \return True if the matching and the pose estimation are OK, false otherwise. */ -bool vpKeyPoint::matchPointAndDetect(const vpImage &I, - const vpCameraParameters &cam, - vpHomogeneousMatrix &cMo, double &error, - double &elapsedTime, vpRect &boundingBox, - vpImagePoint ¢erOfGravity, - bool (*func)(vpHomogeneousMatrix *), +bool vpKeyPoint::matchPointAndDetect(const vpImage &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); + 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 modelImagePoints(m_trainVpPoints.size()); size_t cpt = 0; - for (std::vector::const_iterator it = m_trainVpPoints.begin(); - it != m_trainVpPoints.end(); ++it, cpt++) { + for (std::vector::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); @@ -4037,9 +3613,7 @@ bool vpKeyPoint::matchPointAndDetect(const vpImage &I, // Compute the center of gravity of the current inlier keypoints double meanU = 0.0, meanV = 0.0; - for (std::vector::const_iterator it = - m_ransacInliers.begin(); - it != m_ransacInliers.end(); ++it) { + for (std::vector::const_iterator it = m_ransacInliers.begin(); it != m_ransacInliers.end(); ++it) { meanU += it->get_u(); meanV += it->get_v(); } @@ -4066,11 +3640,10 @@ bool vpKeyPoint::matchPointAndDetect(const vpImage &I, \param listOfAffineI : Optional parameter, list of images after affine transformations */ -void vpKeyPoint::detectExtractAffine( - const vpImage &I, - std::vector > &listOfKeypoints, - std::vector &listOfDescriptors, - std::vector > *listOfAffineI) +void vpKeyPoint::detectExtractAffine(const vpImage &I, + std::vector > &listOfKeypoints, + std::vector &listOfDescriptors, + std::vector > *listOfAffineI) { #if 0 cv::Mat img; @@ -4150,16 +3723,14 @@ void vpKeyPoint::detectExtractAffine( #ifdef VISP_HAVE_OPENMP #pragma omp parallel for #endif - for (int cpt = 0; cpt < static_cast(listOfAffineParams.size()); - cpt++) { + for (int cpt = 0; cpt < static_cast(listOfAffineParams.size()); cpt++) { std::vector 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); + affineSkew(listOfAffineParams[(size_t)cpt].first, listOfAffineParams[(size_t)cpt].second, timg, mask, Ai); if (listOfAffineI != NULL) { cv::Mat img_disp; @@ -4177,8 +3748,7 @@ void vpKeyPoint::detectExtractAffine( cv::waitKey(0); #endif - for (std::map >::const_iterator - it = m_detectors.begin(); + for (std::map >::const_iterator it = m_detectors.begin(); it != m_detectors.end(); ++it) { std::vector kp; it->second->detect(timg, kp, mask); @@ -4254,8 +3824,7 @@ void vpKeyPoint::reset() m_trainPoints.clear(); m_trainVpPoints.clear(); m_useAffineDetection = false; -#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && \ - VISP_HAVE_OPENCV_VERSION < 0x030000) +#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000) m_useBruteForceCrossCheck = true; #endif m_useConsensusPercentage = false; @@ -4278,9 +3847,7 @@ void vpKeyPoint::reset() 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) +void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode, const bool saveTrainingImages) { std::string parent = vpIoTools::getParent(filename); if (!parent.empty()) { @@ -4293,13 +3860,10 @@ void vpKeyPoint::saveLearningData(const std::string &filename, // Save the training image files in the same directory unsigned int cpt = 0; - for (std::map >::const_iterator - it = m_mapOfImages.begin(); - it != m_mapOfImages.end(); ++it, cpt++) { + for (std::map >::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 !"); + throw vpException(vpException::fatalError, "The number of training images to save is too big !"); } std::stringstream ss; @@ -4329,8 +3893,7 @@ void vpKeyPoint::saveLearningData(const std::string &filename, std::string imgFilename = ss.str(); mapOfImgPath[it->first] = imgFilename; - vpImageIo::write(it->second, - parent + (!parent.empty() ? "/" : "") + imgFilename); + vpImageIo::write(it->second, parent + (!parent.empty() ? "/" : "") + imgFilename); } #else std::cout << "Warning: in vpKeyPoint::saveLearningData() training images " @@ -4342,9 +3905,7 @@ void vpKeyPoint::saveLearningData(const std::string &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 !"); + throw vpException(vpException::fatalError, "List of keypoints and list of 3D points have different size !"); } if (binaryMode) { @@ -4360,8 +3921,7 @@ void vpKeyPoint::saveLearningData(const std::string &filename, writeBinaryIntLE(file, nbImgs); #ifdef VISP_HAVE_MODULE_IO - for (std::map::const_iterator it = mapOfImgPath.begin(); - it != mapOfImgPath.end(); ++it) { + for (std::map::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) { // Write image_id int id = it->first; // file.write((char *)(&id), sizeof(id)); @@ -4438,12 +3998,8 @@ void vpKeyPoint::saveLearningData(const std::string &filename, // Write image_id #ifdef VISP_HAVE_MODULE_IO - std::map::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; + std::map::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 @@ -4453,8 +4009,7 @@ void vpKeyPoint::saveLearningData(const std::string &filename, #endif if (have3DInfo) { - float oX = m_trainPoints[i_].x, oY = m_trainPoints[i_].y, - oZ = m_trainPoints[i_].z; + 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); @@ -4477,16 +4032,14 @@ void vpKeyPoint::saveLearningData(const std::string &filename, break; case CV_8S: - file.write((char *)(&m_trainDescriptors.at(i, j)), - sizeof(m_trainDescriptors.at(i, j))); + file.write((char *)(&m_trainDescriptors.at(i, j)), sizeof(m_trainDescriptors.at(i, j))); break; case CV_16U: // file.write((char *)(&m_trainDescriptors.at(i, j)), sizeof(m_trainDescriptors.at(i, j))); - writeBinaryUShortLE( - file, m_trainDescriptors.at(i, j)); + writeBinaryUShortLE(file, m_trainDescriptors.at(i, j)); break; case CV_16S: @@ -4514,8 +4067,7 @@ void vpKeyPoint::saveLearningData(const std::string &filename, break; default: - throw vpException(vpException::fatalError, - "Problem with the data type of descriptors !"); + throw vpException(vpException::fatalError, "Problem with the data type of descriptors !"); break; } } @@ -4525,9 +4077,8 @@ void vpKeyPoint::saveLearningData(const std::string &filename, } 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; + 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 @@ -4547,24 +4098,19 @@ void vpKeyPoint::saveLearningData(const std::string &filename, std::stringstream ss; // Write the training images info - image_node = - xmlNewChild(root_node, NULL, BAD_CAST "TrainingImageInfo", NULL); + image_node = xmlNewChild(root_node, NULL, BAD_CAST "TrainingImageInfo", NULL); #ifdef VISP_HAVE_MODULE_IO - for (std::map::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()); + for (std::map::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()); + 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); + 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(); @@ -4572,100 +4118,80 @@ void vpKeyPoint::saveLearningData(const std::string &filename, // Write the number of rows ss.str(""); ss << nRows; - xmlNewChild(descriptors_info_node, NULL, BAD_CAST "nrows", - BAD_CAST ss.str().c_str()); + 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()); + 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()); + 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); + 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()); + 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()); + 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()); + 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()); + 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 << 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()); + 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()); + xmlNewChild(descriptor_node, NULL, BAD_CAST "class_id", BAD_CAST ss.str().c_str()); ss.str(""); #ifdef VISP_HAVE_MODULE_IO - std::map::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()); + std::map::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()); + 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()); + 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()); + 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()); + xmlNewChild(descriptor_node, NULL, BAD_CAST "oZ", BAD_CAST ss.str().c_str()); } desc_node = xmlNewChild(descriptor_node, NULL, BAD_CAST "desc", NULL); @@ -4706,23 +4232,19 @@ void vpKeyPoint::saveLearningData(const std::string &filename, case CV_32F: // max_digits10 == 9 for float - ss << std::fixed << std::setprecision(9) - << m_trainDescriptors.at(i, j); + ss << std::fixed << std::setprecision(9) << m_trainDescriptors.at(i, j); break; case CV_64F: // max_digits10 == 17 for double - ss << std::fixed << std::setprecision(17) - << m_trainDescriptors.at(i, j); + ss << std::fixed << std::setprecision(17) << m_trainDescriptors.at(i, j); break; default: - throw vpException(vpException::fatalError, - "Problem with the data type of descriptors !"); + 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()); + xmlNewChild(desc_node, NULL, BAD_CAST "val", BAD_CAST ss.str().c_str()); } } @@ -4746,24 +4268,16 @@ void vpKeyPoint::saveLearningData(const std::string &filename, // 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; - } + 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; - } + 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 &keypoints, int n_points) +void vpKeyPoint::KeyPointsFilter::retainBest(std::vector &keypoints, int n_points) { // this is only necessary if the keypoints size is greater than the number // of desired points. @@ -4774,15 +4288,13 @@ void vpKeyPoint::KeyPointsFilter::retainBest( } // 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()); + 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::const_iterator new_end = std::partition( - keypoints.begin() + n_points, keypoints.end(), - KeypointResponseGreaterThanThreshold(ambiguous_response)); + 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())); @@ -4792,37 +4304,28 @@ void vpKeyPoint::KeyPointsFilter::retainBest( struct RoiPredicate { RoiPredicate(const cv::Rect &_r) : r(_r) {} - bool operator()(const cv::KeyPoint &keyPt) const - { - return !r.contains(keyPt.pt); - } + bool operator()(const cv::KeyPoint &keyPt) const { return !r.contains(keyPt.pt); } cv::Rect r; }; -void vpKeyPoint::KeyPointsFilter::runByImageBorder( - std::vector &keypoints, cv::Size imageSize, int borderSize) +void vpKeyPoint::KeyPointsFilter::runByImageBorder(std::vector &keypoints, cv::Size imageSize, + int borderSize) { if (borderSize > 0) { - if (imageSize.height <= borderSize * 2 || - imageSize.width <= borderSize * 2) + 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()); + 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) - { - } + SizePredicate(float _minSize, float _maxSize) : minSize(_minSize), maxSize(_maxSize) {} bool operator()(const cv::KeyPoint &keyPt) const { @@ -4833,16 +4336,13 @@ struct SizePredicate { float minSize, maxSize; }; -void vpKeyPoint::KeyPointsFilter::runByKeypointSize( - std::vector &keypoints, float minSize, float maxSize) +void vpKeyPoint::KeyPointsFilter::runByKeypointSize(std::vector &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()); + keypoints.erase(std::remove_if(keypoints.begin(), keypoints.end(), SizePredicate(minSize, maxSize)), keypoints.end()); } class MaskPredicate @@ -4851,8 +4351,7 @@ class MaskPredicate MaskPredicate(const cv::Mat &_mask) : mask(_mask) {} bool operator()(const cv::KeyPoint &key_pt) const { - return mask.at((int)(key_pt.pt.y + 0.5f), - (int)(key_pt.pt.x + 0.5f)) == 0; + return mask.at((int)(key_pt.pt.y + 0.5f), (int)(key_pt.pt.x + 0.5f)) == 0; } private: @@ -4860,15 +4359,12 @@ class MaskPredicate MaskPredicate &operator=(const MaskPredicate &); }; -void vpKeyPoint::KeyPointsFilter::runByPixelsMask( - std::vector &keypoints, const cv::Mat &mask) +void vpKeyPoint::KeyPointsFilter::runByPixelsMask(std::vector &keypoints, const cv::Mat &mask) { if (mask.empty()) return; - keypoints.erase( - std::remove_if(keypoints.begin(), keypoints.end(), MaskPredicate(mask)), - keypoints.end()); + keypoints.erase(std::remove_if(keypoints.begin(), keypoints.end(), MaskPredicate(mask)), keypoints.end()); } struct KeyPoint_LessThan { @@ -4877,38 +4373,33 @@ struct KeyPoint_LessThan { { 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::epsilon())) { // if (kp1.pt.x != - // kp2.pt.x) { + if (!vpMath::equal(kp1.pt.x, kp2.pt.x, + std::numeric_limits::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::epsilon())) { // if (kp1.pt.y != - // kp2.pt.y) { + if (!vpMath::equal(kp1.pt.y, kp2.pt.y, + std::numeric_limits::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::epsilon())) { // if (kp1.size != - // kp2.size) { + if (!vpMath::equal(kp1.size, kp2.size, + std::numeric_limits::epsilon())) { // if (kp1.size != + // kp2.size) { return kp1.size > kp2.size; } - if (!vpMath::equal( - kp1.angle, kp2.angle, - std::numeric_limits::epsilon())) { // if (kp1.angle != - // kp2.angle) { + if (!vpMath::equal(kp1.angle, kp2.angle, + std::numeric_limits::epsilon())) { // if (kp1.angle != + // kp2.angle) { return kp1.angle < kp2.angle; } - if (!vpMath::equal( - kp1.response, kp2.response, - std::numeric_limits::epsilon())) { // if (kp1.response != - // kp2.response) { + if (!vpMath::equal(kp1.response, kp2.response, + std::numeric_limits::epsilon())) { // if (kp1.response != + // kp2.response) { return kp1.response > kp2.response; } @@ -4925,8 +4416,7 @@ struct KeyPoint_LessThan { const std::vector *kp; }; -void vpKeyPoint::KeyPointsFilter::removeDuplicated( - std::vector &keypoints) +void vpKeyPoint::KeyPointsFilter::removeDuplicated(std::vector &keypoints) { size_t i, j, n = keypoints.size(); std::vector kpidx(n); @@ -4941,14 +4431,10 @@ void vpKeyPoint::KeyPointsFilter::removeDuplicated( 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::epsilon()) || - !vpMath::equal(kp1.pt.y, kp2.pt.y, - std::numeric_limits::epsilon()) || - !vpMath::equal(kp1.size, kp2.size, - std::numeric_limits::epsilon()) || - !vpMath::equal(kp1.angle, kp2.angle, - std::numeric_limits::epsilon())) { + if (!vpMath::equal(kp1.pt.x, kp2.pt.x, std::numeric_limits::epsilon()) || + !vpMath::equal(kp1.pt.y, kp2.pt.y, std::numeric_limits::epsilon()) || + !vpMath::equal(kp1.size, kp2.size, std::numeric_limits::epsilon()) || + !vpMath::equal(kp1.angle, kp2.angle, std::numeric_limits::epsilon())) { j = i; } else { mask[kpidx[i]] = 0; @@ -4969,8 +4455,8 @@ void vpKeyPoint::KeyPointsFilter::removeDuplicated( /* * PyramidAdaptedFeatureDetector */ -vpKeyPoint::PyramidAdaptedFeatureDetector::PyramidAdaptedFeatureDetector( - const cv::Ptr &_detector, int _maxLevel) +vpKeyPoint::PyramidAdaptedFeatureDetector::PyramidAdaptedFeatureDetector(const cv::Ptr &_detector, + int _maxLevel) : detector(_detector), maxLevel(_maxLevel) { } @@ -4980,16 +4466,14 @@ bool vpKeyPoint::PyramidAdaptedFeatureDetector::empty() const return detector.empty() || (cv::FeatureDetector *)detector->empty(); } -void vpKeyPoint::PyramidAdaptedFeatureDetector::detect( - cv::InputArray image, CV_OUT std::vector &keypoints, - cv::InputArray mask) +void vpKeyPoint::PyramidAdaptedFeatureDetector::detect(cv::InputArray image, + CV_OUT std::vector &keypoints, cv::InputArray mask) { detectImpl(image.getMat(), keypoints, mask.getMat()); } -void vpKeyPoint::PyramidAdaptedFeatureDetector::detectImpl( - const cv::Mat &image, std::vector &keypoints, - const cv::Mat &mask) const +void vpKeyPoint::PyramidAdaptedFeatureDetector::detectImpl(const cv::Mat &image, std::vector &keypoints, + const cv::Mat &mask) const { cv::Mat src = image; cv::Mat src_mask = mask; @@ -5006,8 +4490,7 @@ void vpKeyPoint::PyramidAdaptedFeatureDetector::detectImpl( // Detect on current level of the pyramid std::vector new_pts; detector->detect(src, new_pts, src_mask); - std::vector::iterator it = new_pts.begin(), - end = new_pts.end(); + std::vector::iterator it = new_pts.begin(), end = new_pts.end(); for (; it != end; ++it) { it->pt.x *= multiplier; it->pt.y *= multiplier; diff --git a/modules/vision/src/key-point/vpKeyPointSurf.cpp b/modules/vision/src/key-point/vpKeyPointSurf.cpp index 5190881345..e06107e3e1 100644 --- a/modules/vision/src/key-point/vpKeyPointSurf.cpp +++ b/modules/vision/src/key-point/vpKeyPointSurf.cpp @@ -38,8 +38,7 @@ #include -#if defined(VISP_HAVE_OPENCV_NONFREE) && \ - (VISP_HAVE_OPENCV_VERSION >= 0x010100) && \ +#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 @@ -53,20 +52,14 @@ #include #include -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 &ptpairs); +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 &ptpairs); // Compare two surf descriptors. -double compareSURFDescriptors(const float *d1, const float *d2, double best, - int length) +double compareSURFDescriptors(const float *d1, const float *d2, double best, int length) { double total_cost = 0; int i; @@ -85,9 +78,7 @@ double compareSURFDescriptors(const float *d1, const float *d2, double best, // 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 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; @@ -118,8 +109,7 @@ int naiveNearestNeighbor(const float *vec, int laplacian, // 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 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; @@ -146,9 +136,8 @@ int naiveNearestNeighbor(const float *vec, const CvSeq *ref_keypoints, } // Find all the matched points -void findPairs(const CvSeq *objectKeypoints, const CvSeq *objectDescriptors, - const CvSeq *imageKeypoints, const CvSeq *imageDescriptors, - std::vector &ptpairs) +void findPairs(const CvSeq *objectKeypoints, const CvSeq *objectDescriptors, const CvSeq *imageKeypoints, + const CvSeq *imageDescriptors, std::vector &ptpairs) { int i; CvSeqReader reader, kreader; @@ -161,8 +150,7 @@ void findPairs(const CvSeq *objectKeypoints, const CvSeq *objectDescriptors, 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); + int nearest_neighbor = naiveNearestNeighbor(descriptor, kp->laplacian, imageKeypoints, imageDescriptors); if (nearest_neighbor >= 0) { ptpairs.push_back(i); ptpairs.push_back(nearest_neighbor); @@ -178,10 +166,8 @@ void findPairs(const CvSeq *objectKeypoints, const CvSeq *objectDescriptors, */ 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) + : vpBasicKeyPoint(), storage(NULL), params(), storage_cur(NULL), image_keypoints(NULL), image_descriptors(NULL), + ref_keypoints(NULL), ref_descriptors(NULL), hessianThreshold(500), descriptorType(extendedDescriptor) { init(); } @@ -275,30 +261,23 @@ unsigned int vpKeyPointSurf::buildReference(const vpImage &I) \return the number of reference points. */ -unsigned int vpKeyPointSurf::buildReference(const vpImage &I, - const vpImagePoint &iP, - const unsigned int height, - const unsigned int width) +unsigned int vpKeyPointSurf::buildReference(const vpImage &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()) { + 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")); + throw(vpException(vpImageException::notInTheImage, "Bad size for the subimage")); } vpImage subImage; - vpImageTools::crop(I, (unsigned int)iP.get_i(), (unsigned int)iP.get_j(), - height, width, subImage); + vpImageTools::crop(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()); + (referenceImagePointsList[k]).set_i((referenceImagePointsList[k]).get_i() + iP.get_i()); + (referenceImagePointsList[k]).set_j((referenceImagePointsList[k]).get_j() + iP.get_j()); } return (nbRefPoint); } @@ -316,14 +295,12 @@ unsigned int vpKeyPointSurf::buildReference(const vpImage &I, \return the number of reference points. */ -unsigned int vpKeyPointSurf::buildReference(const vpImage &I, - const vpRect &rectangle) +unsigned int vpKeyPointSurf::buildReference(const vpImage &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())); + return (this->buildReference(I, iP, (unsigned int)rectangle.getHeight(), (unsigned int)rectangle.getWidth())); } /*! @@ -358,8 +335,7 @@ unsigned int vpKeyPointSurf::matchPoint(const vpImage &I) } storage_cur = cvCreateMemStorage(0); - cvExtractSURF(currentImage, 0, &image_keypoints, &image_descriptors, - storage_cur, params); + cvExtractSURF(currentImage, 0, &image_keypoints, &image_descriptors, storage_cur, params); CvSeqReader reader, kreader; cvStartReadSeq(ref_keypoints, &kreader); @@ -375,8 +351,7 @@ unsigned int vpKeyPointSurf::matchPoint(const vpImage &I) 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); + int nearest_neighbor = naiveNearestNeighbor(descriptor, kp->laplacian, image_keypoints, image_descriptors); if (nearest_neighbor >= 0) { indexReferencePair.push_back(i); indexImagePair.push_back(nearest_neighbor); @@ -385,8 +360,7 @@ unsigned int vpKeyPointSurf::matchPoint(const vpImage &I) } std::list::const_iterator indexImagePairIter = indexImagePair.begin(); - std::list::const_iterator indexReferencePairIter = - indexReferencePair.begin(); + std::list::const_iterator indexReferencePairIter = indexReferencePair.begin(); matchedReferencePoints.resize(0); @@ -438,30 +412,23 @@ unsigned int vpKeyPointSurf::matchPoint(const vpImage &I) \return the number of point which have been matched. */ -unsigned int vpKeyPointSurf::matchPoint(const vpImage &I, - const vpImagePoint &iP, - const unsigned int height, - const unsigned int width) +unsigned int vpKeyPointSurf::matchPoint(const vpImage &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()) { + 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")); + throw(vpException(vpImageException::notInTheImage, "Bad size for the subimage")); } vpImage subImage; - vpImageTools::crop(I, (unsigned int)iP.get_i(), (unsigned int)iP.get_j(), - height, width, subImage); + vpImageTools::crop(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()); + (currentImagePointsList[k]).set_i((currentImagePointsList[k]).get_i() + iP.get_i()); + (currentImagePointsList[k]).set_j((currentImagePointsList[k]).get_j() + iP.get_j()); } return (nbMatchedPoint); @@ -481,14 +448,12 @@ unsigned int vpKeyPointSurf::matchPoint(const vpImage &I, \return the number of point which have been matched. */ -unsigned int vpKeyPointSurf::matchPoint(const vpImage &I, - const vpRect &rectangle) +unsigned int vpKeyPointSurf::matchPoint(const vpImage &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())); + return (this->matchPoint(I, iP, (unsigned int)rectangle.getHeight(), (unsigned int)rectangle.getWidth())); } /*! @@ -510,8 +475,7 @@ unsigned int vpKeyPointSurf::matchPoint(const vpImage &I, points. */ -void vpKeyPointSurf::display(const vpImage &Ireference, - const vpImage &Icurrent, +void vpKeyPointSurf::display(const vpImage &Ireference, const vpImage &Icurrent, unsigned int size) { // matchedPointsCurrentImageList.front(); @@ -526,11 +490,8 @@ void vpKeyPointSurf::display(const vpImage &Ireference, // } 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); + vpDisplay::displayCross(Ireference, referenceImagePointsList[matchedReferencePoints[i]], size, vpColor::red); + vpDisplay::displayCross(Icurrent, currentImagePointsList[i], size, vpColor::green); // matchedPointsReferenceImageList.next(); // matchedPointsCurrentImageList.next(); } @@ -549,8 +510,7 @@ void vpKeyPointSurf::display(const vpImage &Ireference, \param color : Color used to display the matched points. */ -void vpKeyPointSurf::display(const vpImage &Icurrent, - unsigned int size, const vpColor &color) +void vpKeyPointSurf::display(const vpImage &Icurrent, unsigned int size, const vpColor &color) { // matchedPointsCurrentImageList.front(); // @@ -561,9 +521,7 @@ void vpKeyPointSurf::display(const vpImage &Icurrent, } } -std::list * -vpKeyPointSurf::matchPoint(std::list descriptorList, - std::list laplacianList) +std::list *vpKeyPointSurf::matchPoint(std::list descriptorList, std::list laplacianList) { std::list *pairPoints = new std::list; @@ -575,16 +533,14 @@ vpKeyPointSurf::matchPoint(std::list descriptorList, CvSeqReader reader; cvStartReadSeq(ref_descriptors, &reader); - std::list::const_iterator descriptorListIter = - descriptorList.begin(); + std::list::const_iterator descriptorListIter = descriptorList.begin(); std::list::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); + int nearest_neighbor = naiveNearestNeighbor(descriptor, *laplacianListIter, ref_keypoints, ref_descriptors); if (nearest_neighbor >= 0) { int *tab; @@ -609,11 +565,9 @@ vpKeyPointSurf::matchPoint(std::list descriptorList, */ float *vpKeyPointSurf::getDescriptorReferencePoint(const int index) { - if (index >= static_cast(referenceImagePointsList.size()) || - index < 0) { + if (index >= static_cast(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")); + throw(vpException(vpException::fatalError, "Index of the reference point out of range")); } float *descriptor = NULL; @@ -640,11 +594,9 @@ float *vpKeyPointSurf::getDescriptorReferencePoint(const int index) */ int vpKeyPointSurf::getLaplacianReferencePoint(const int index) { - if (index >= static_cast(referenceImagePointsList.size()) || - index < 0) { + if (index >= static_cast(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")); + throw(vpException(vpException::fatalError, "Index of the reference point out of range")); } CvSeqReader reader; @@ -672,14 +624,11 @@ int vpKeyPointSurf::getLaplacianReferencePoint(const int index) 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) +void vpKeyPointSurf::getDescriptorParamReferencePoint(const int index, int &size, float &dir) { - if (index >= static_cast(referenceImagePointsList.size()) || - index < 0) { + if (index >= static_cast(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")); + throw(vpException(vpException::fatalError, "Index of the reference point out of range")); } CvSeqReader reader; diff --git a/modules/vision/src/key-point/vpPlanarObjectDetector.cpp b/modules/vision/src/key-point/vpPlanarObjectDetector.cpp index ecf566a1a9..36ec70e202 100644 --- a/modules/vision/src/key-point/vpPlanarObjectDetector.cpp +++ b/modules/vision/src/key-point/vpPlanarObjectDetector.cpp @@ -38,9 +38,8 @@ #include -#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 >= 0x020000) && \ + (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0 #include #include @@ -58,8 +57,8 @@ */ vpPlanarObjectDetector::vpPlanarObjectDetector() - : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), - modelROI(), currentImagePoints(), refImagePoints(), minNbMatching(10) + : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(), + refImagePoints(), minNbMatching(10) { } @@ -71,10 +70,9 @@ vpPlanarObjectDetector::vpPlanarObjectDetector() \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) +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); } @@ -98,12 +96,10 @@ vpPlanarObjectDetector::~vpPlanarObjectDetector() {} \param ip : the list of image point. \param nbpt : the number of point. */ -void vpPlanarObjectDetector::computeRoi(vpImagePoint *ip, - const unsigned int nbpt) +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."); + throw vpException(vpException::badValue, "Not enough point to compute the region of interest."); } std::vector ptsx(nbpt); @@ -142,8 +138,7 @@ void vpPlanarObjectDetector::computeRoi(vpImagePoint *ip, \return The number of reference points. */ -unsigned int -vpPlanarObjectDetector::buildReference(const vpImage &_I) +unsigned int vpPlanarObjectDetector::buildReference(const vpImage &_I) { modelROI.x = 0; modelROI.y = 0; @@ -168,9 +163,8 @@ vpPlanarObjectDetector::buildReference(const vpImage &_I) \return the number of reference points */ -unsigned int vpPlanarObjectDetector::buildReference( - const vpImage &_I, const vpImagePoint &_iP, - unsigned int _height, unsigned int _width) +unsigned int vpPlanarObjectDetector::buildReference(const vpImage &_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(); @@ -193,9 +187,7 @@ unsigned int vpPlanarObjectDetector::buildReference( \return The number of reference points. */ -unsigned int -vpPlanarObjectDetector::buildReference(const vpImage &_I, - const vpRect &_rectangle) +unsigned int vpPlanarObjectDetector::buildReference(const vpImage &_I, const vpRect &_rectangle) { unsigned int res = fern.buildReference(_I, _rectangle); @@ -253,8 +245,7 @@ bool vpPlanarObjectDetector::matchPoint(const vpImage &I) /* part of code from OpenCV planarObjectDetector */ std::vector mask; - H = cv::findHomography(cv::Mat(refPts), cv::Mat(curPts), mask, cv::RANSAC, - 10); + H = cv::findHomography(cv::Mat(refPts), cv::Mat(curPts), mask, cv::RANSAC, 10); if (H.data) { const cv::Mat_ &H_tmp = H; @@ -263,11 +254,8 @@ bool vpPlanarObjectDetector::matchPoint(const vpImage &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)); + 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; @@ -313,22 +301,17 @@ bool vpPlanarObjectDetector::matchPoint(const vpImage &I) \return true if the surface has been found. */ -bool vpPlanarObjectDetector::matchPoint(const vpImage &I, - const vpImagePoint &iP, - const unsigned int height, - const unsigned int width) +bool vpPlanarObjectDetector::matchPoint(const vpImage &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()) { + 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")); + throw(vpException(vpImageException::notInTheImage, "Bad size for the subimage")); } vpImage subImage; - vpImageTools::crop(I, (unsigned int)iP.get_i(), (unsigned int)iP.get_j(), - height, width, subImage); + vpImageTools::crop(I, (unsigned int)iP.get_i(), (unsigned int)iP.get_j(), height, width, subImage); return this->matchPoint(subImage); } @@ -344,14 +327,12 @@ bool vpPlanarObjectDetector::matchPoint(const vpImage &I, \return True if the surface has been found. */ -bool vpPlanarObjectDetector::matchPoint(const vpImage &I, - const vpRect &rectangle) +bool vpPlanarObjectDetector::matchPoint(const vpImage &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())); + return (this->matchPoint(I, iP, (unsigned int)rectangle.getHeight(), (unsigned int)rectangle.getWidth())); } /*! @@ -364,22 +345,18 @@ bool vpPlanarObjectDetector::matchPoint(const vpImage &I, \param displayKpts : The flag to display keypoints in addition to the surface. */ -void vpPlanarObjectDetector::display(vpImage &I, - bool displayKpts) +void vpPlanarObjectDetector::display(vpImage &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 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); + 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); + vpImagePoint ip(currentImagePoints[i].get_i() - modelROI.y, currentImagePoints[i].get_j() - modelROI.x); vpDisplay::displayCross(I, ip, 5, vpColor::red); } } @@ -402,9 +379,7 @@ void vpPlanarObjectDetector::display(vpImage &I, \param displayKpts : The flag to display keypoints in addition to the surface. */ -void vpPlanarObjectDetector::display(vpImage &Iref, - vpImage &Icurrent, - bool displayKpts) +void vpPlanarObjectDetector::display(vpImage &Iref, vpImage &Icurrent, bool displayKpts) { display(Icurrent, displayKpts); @@ -425,8 +400,7 @@ void vpPlanarObjectDetector::display(vpImage &Iref, It can have any file extension. \param objName : The name of the object. */ -void vpPlanarObjectDetector::load(const std::string &dataFilename, - const std::string &objName) +void vpPlanarObjectDetector::load(const std::string &dataFilename, const std::string &objName) { fern.load(dataFilename, objName); modelROI = fern.getModelROI(); @@ -440,8 +414,7 @@ void vpPlanarObjectDetector::load(const std::string &dataFilename, \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) +void vpPlanarObjectDetector::recordDetector(const std::string &objectName, const std::string &dataFile) { fern.record(objectName, dataFile); } @@ -490,24 +463,20 @@ void vpPlanarObjectDetector::initialiseRefCorners(const cv::Rect &_modelROI) ref_corners.push_back(ip); } -void vpPlanarObjectDetector::getReferencePoint(unsigned int _i, - vpImagePoint &_imPoint) +void vpPlanarObjectDetector::getReferencePoint(unsigned int _i, vpImagePoint &_imPoint) { if (_i >= refImagePoints.size()) { - throw vpException(vpException::fatalError, - "index out of bound in getMatchedPoints."); + throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints."); } _imPoint = refImagePoints[_i]; } -void vpPlanarObjectDetector::getMatchedPoints(const unsigned int _index, - vpImagePoint &_referencePoint, +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."); + throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints."); } _referencePoint = refImagePoints[_index]; diff --git a/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp b/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp index 4cd2ba24e1..b2bfb0c278 100644 --- a/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp +++ b/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp @@ -51,13 +51,10 @@ #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) + : 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(); } @@ -75,8 +72,7 @@ void vpXmlConfigParserKeyPoint::init() nodeMap["matcher"] = matcher; nodeMap["name"] = name; nodeMap["matching_method"] = matching_method; - nodeMap["constantFactorDistanceThreshold"] = - constant_factor_distance_threshold; + nodeMap["constantFactorDistanceThreshold"] = constant_factor_distance_threshold; nodeMap["stdDistanceThreshold"] = std_distance_threshold; nodeMap["ratioDistanceThreshold"] = ratio_distance_threshold; nodeMap["stdAndRatioDistanceThreshold"] = std_and_ratio_distance_threshold; @@ -97,10 +93,7 @@ void vpXmlConfigParserKeyPoint::init() 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); -} +void vpXmlConfigParserKeyPoint::parse(const std::string &filename) { vpXmlParser::parse(filename); } /*! Read the parameters of the class from the file given by its document pointer @@ -115,11 +108,9 @@ void vpXmlConfigParserKeyPoint::readMainClass(xmlDocPtr doc, xmlNodePtr node) bool extractor_node = false; bool matcher_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case detector: @@ -149,18 +140,15 @@ void vpXmlConfigParserKeyPoint::readMainClass(xmlDocPtr doc, xmlNodePtr node) } if (!detector_node) { - std::cout << "detector: name: " << m_detectorName << " (default)" - << std::endl; + std::cout << "detector: name: " << m_detectorName << " (default)" << std::endl; } if (!extractor_node) { - std::cout << "extractor: name: " << m_extractorName << " (default)" - << std::endl; + std::cout << "extractor: name: " << m_extractorName << " (default)" << std::endl; } if (!matcher_node) { - std::cout << "matcher: name: " << m_matcherName << " (default)" - << std::endl; + std::cout << "matcher: name: " << m_matcherName << " (default)" << std::endl; } } @@ -174,11 +162,9 @@ void vpXmlConfigParserKeyPoint::read_detector(xmlDocPtr doc, xmlNodePtr node) { bool detector_name_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case name: @@ -194,8 +180,7 @@ void vpXmlConfigParserKeyPoint::read_detector(xmlDocPtr doc, xmlNodePtr node) } if (!detector_name_node) - std::cout << "detector : Name : " << m_detectorName << " (default)" - << std::endl; + std::cout << "detector : Name : " << m_detectorName << " (default)" << std::endl; else std::cout << "detector : Name : " << m_detectorName << std::endl; } @@ -210,11 +195,9 @@ void vpXmlConfigParserKeyPoint::read_extractor(xmlDocPtr doc, xmlNodePtr node) { bool extractor_name_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case name: @@ -230,8 +213,7 @@ void vpXmlConfigParserKeyPoint::read_extractor(xmlDocPtr doc, xmlNodePtr node) } if (!extractor_name_node) - std::cout << "extractor : Name : " << m_extractorName << " (default)" - << std::endl; + std::cout << "extractor : Name : " << m_extractorName << " (default)" << std::endl; else std::cout << "extractor : Name : " << m_extractorName << std::endl; } @@ -250,11 +232,9 @@ void vpXmlConfigParserKeyPoint::read_matcher(xmlDocPtr doc, xmlNodePtr node) bool matching_factor_threshold_node = false; bool matching_ratio_threshold_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case name: @@ -265,8 +245,7 @@ void vpXmlConfigParserKeyPoint::read_matcher(xmlDocPtr doc, xmlNodePtr node) case matching_method: { matchingMethodName = xmlReadStringChild(doc, dataNode); - std::map::iterator iter_data2 = - nodeMap.find(matchingMethodName); + std::map::iterator iter_data2 = nodeMap.find(matchingMethodName); if (iter_data2 != nodeMap.end()) { matching_method_node = true; switch (iter_data2->second) { @@ -316,31 +295,24 @@ void vpXmlConfigParserKeyPoint::read_matcher(xmlDocPtr doc, xmlNodePtr node) } if (!matcher_name_node) - std::cout << "matcher : Name : " << m_matcherName << " (default)" - << std::endl; + 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; + std::cout << "matcher : Filter method : " << matchingMethodName << " (default)" << std::endl; else - std::cout << "matcher : Filter method : " << matchingMethodName - << std::endl; + std::cout << "matcher : Filter method : " << matchingMethodName << std::endl; if (!matching_factor_threshold_node) - std::cout << "matcher : matching factor threshold : " - << m_matchingFactorThreshold << " (default)" << std::endl; + std::cout << "matcher : matching factor threshold : " << m_matchingFactorThreshold << " (default)" << std::endl; else - std::cout << "matcher : matching factor threshold : " - << m_matchingFactorThreshold << std::endl; + 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; + std::cout << "matcher : matching ratio threshold : " << m_matchingRatioThreshold << " (default)" << std::endl; else - std::cout << "matcher : matching ratio threshold : " - << m_matchingRatioThreshold << std::endl; + std::cout << "matcher : matching ratio threshold : " << m_matchingRatioThreshold << std::endl; } /*! @@ -359,11 +331,9 @@ void vpXmlConfigParserKeyPoint::read_ransac(xmlDocPtr doc, xmlNodePtr node) bool ransac_threshold_node = false; bool ransac_consensus_percentage_node = false; - for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; - dataNode = dataNode->next) { + for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) { if (dataNode->type == XML_ELEMENT_NODE) { - std::map::iterator iter_data = - this->nodeMap.find((char *)dataNode->name); + std::map::iterator iter_data = this->nodeMap.find((char *)dataNode->name); if (iter_data != nodeMap.end()) { switch (iter_data->second) { case use_ransac_vvs: @@ -372,8 +342,7 @@ void vpXmlConfigParserKeyPoint::read_ransac(xmlDocPtr doc, xmlNodePtr node) break; case use_ransac_consensus_percentage: - m_useRansacConsensusPercentage = - xmlReadIntChild(doc, dataNode) != 0; + m_useRansacConsensusPercentage = xmlReadIntChild(doc, dataNode) != 0; use_ransac_consensus_percentage_node = true; break; @@ -410,25 +379,19 @@ void vpXmlConfigParserKeyPoint::read_ransac(xmlDocPtr doc, xmlNodePtr node) } if (!use_ransac_vvs_node) - std::cout << "ransac: use ransac vvs pose estimation: " << m_useRansacVVS - << " (default)" << std::endl; + 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; + 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; + std::cout << "ransac: use consensus percentage: " << m_useRansacConsensusPercentage << " (default)" << std::endl; else - std::cout << "ransac: use consensus percentage: " - << m_useRansacConsensusPercentage << std::endl; + 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; + std::cout << "ransac: nb ransac iterations: " << m_nbRansacIterations << " (default)" << std::endl; else - std::cout << "ransac: nb ransac iterations: " << m_nbRansacIterations - << std::endl; + 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 " @@ -440,25 +403,20 @@ void vpXmlConfigParserKeyPoint::read_ransac(xmlDocPtr doc, xmlNodePtr node) << m_ransacReprojectionError << std::endl; if (!nb_ransac_min_inlier_count_node) - std::cout << "ransac: nb ransac min inlier count: " - << m_nbRansacMinInlierCount << " (default)" << std::endl; + 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; + 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; + 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; + 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; + std::cout << "ransac: consensus percentage: " << m_ransacConsensusPercentage << " (default)" << std::endl; else - std::cout << "ransac: consensus percentage: " - << m_ransacConsensusPercentage << std::endl; + std::cout << "ransac: consensus percentage: " << m_ransacConsensusPercentage << std::endl; } #elif !defined(VISP_BUILD_SHARED_LIBS) diff --git a/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp b/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp index 75c014c1b9..2a28d72d3a 100644 --- a/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp +++ b/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp @@ -46,11 +46,11 @@ #include #define SIGN(x) ((x) < 0 ? -1 : 1) -#define SWAP(a, b, c) \ - { \ - (c) = (a); \ - (a) = (b); \ - (b) = (c); \ +#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 @@ -223,9 +223,8 @@ double enorm(const double *x, int n) * 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) +int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *delta, double *par, double *x, + double *sdiag, double *wa1, double *wa2) { const double tol1 = 0.1; /* tolerance a 0.1 */ @@ -247,8 +246,7 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, 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::epsilon() && - nsing == n) + if (std::fabs(*pt) <= std::numeric_limits::epsilon() && nsing == n) nsing = i - 1; if (nsing < n) @@ -392,8 +390,7 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, // temp) // && (temp < 0.0)) || (iter == 10)) if ((std::fabs(fp) <= tol1 * (*delta)) || - ((std::fabs(parl) <= std::numeric_limits::epsilon()) && - (fp <= temp) && (temp < 0.0)) || + ((std::fabs(parl) <= std::numeric_limits::epsilon()) && (fp <= temp) && (temp < 0.0)) || (iter == 10)) { // terminaison. @@ -478,13 +475,11 @@ double pythag(double a, double b) return (pyth); } - r = ((std::min)(std::fabs(a), std::fabs(b)) / p) * - ((std::min)(std::fabs(a), std::fabs(b)) / p); + 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::epsilon()) { + while (std::fabs(t - 4.0) < std::fabs(vpMath::maximum(t, 4.0)) * std::numeric_limits::epsilon()) { double s = r / t; double u = 1.0 + 2.0 * s; p *= u; @@ -541,8 +536,8 @@ double pythag(double a, double b) * 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) +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; @@ -627,13 +622,11 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, *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::epsilon())) { + if (pivot && (std::fabs(rdiag[k]) > std::numeric_limits::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) { + 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]; } @@ -705,8 +698,7 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, *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 qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag, double *wa) { int i, j, k, kp1, l; /* compteur de boucle */ int nsing; @@ -817,8 +809,7 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, for (i = 0; i < n; i++) { // if (sdiag[i] == 0.0 && nsing == n) - if ((std::fabs(sdiag[i]) <= std::numeric_limits::epsilon()) && - nsing == n) + if ((std::fabs(sdiag[i]) <= std::numeric_limits::epsilon()) && nsing == n) nsing = i - 1; if (nsing < n) @@ -955,16 +946,12 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, * 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) +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; + 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; @@ -1000,8 +987,8 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, 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)) { + 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. */ @@ -1248,8 +1235,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, /* * determination du parametre de Levenberg-Marquardt. */ - lmpar(n, fjac, ldfjac, ipvt, diag, qtf, &delta, &par, wa1, wa2, wa3, - wa4); + 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. @@ -1338,8 +1324,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, if (ratio > tol25) { // if ((par == 0.0) || (ratio <= tol75)) - if ((std::fabs(par) <= std::numeric_limits::epsilon()) || - (ratio <= tol75)) { + if ((std::fabs(par) <= std::numeric_limits::epsilon()) || (ratio <= tol75)) { delta = pnorm / tol5; par *= tol5; } @@ -1383,15 +1368,13 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, * tests pour convergence. */ - if ((std::fabs(actred) <= ftol) && (prered <= ftol) && - (tol5 * ratio <= 1.0)) + 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) + if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0) && *info == 2) *info = 3; if (*info != 0) { @@ -1416,8 +1399,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, if (*nfev >= maxfev) *info = 5; - if ((std::fabs(actred) <= epsmch) && (prered <= epsmch) && - (tol5 * ratio <= 1.0)) + if ((std::fabs(actred) <= epsmch) && (prered <= epsmch) && (tol5 * ratio <= 1.0)) *info = 6; if (delta <= epsmch * xnorm) @@ -1526,10 +1508,8 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, * 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) +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; @@ -1540,10 +1520,8 @@ int lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, /* 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))); + 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); } @@ -1556,9 +1534,8 @@ int lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, 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]); + 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; diff --git a/modules/vision/src/pose-estimation/vpPose.cpp b/modules/vision/src/pose-estimation/vpPose.cpp index e60e7f2bfc..c1d3346795 100644 --- a/modules/vision/src/pose-estimation/vpPose.cpp +++ b/modules/vision/src/pose-estimation/vpPose.cpp @@ -92,14 +92,11 @@ void vpPose::init() /*! Default 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), - ransacFlags(PREFILTER_DUPLICATE_POINTS), listOfPoints(), + : 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), ransacFlags(PREFILTER_DUPLICATE_POINTS), listOfPoints(), useParallelRansac(false), - nbParallelRansacThreads( - 0), // 0 means that OpenMP is used to get the number of CPU threads + nbParallelRansacThreads(0), // 0 means that OpenMP is used to get the number of CPU threads vvsEpsilon(1e-8) { } @@ -159,10 +156,7 @@ void vpPose::addPoints(const std::vector &lP) npt = (unsigned int)listP.size(); } -void vpPose::setDistanceToPlaneForCoplanarityTest(double d) -{ - distanceToPlaneForCoplanarityTest = d; -} +void vpPose::setDistanceToPlaneForCoplanarityTest(double d) { distanceToPlaneForCoplanarityTest = d; } /*! Test the coplanarity of the set of points @@ -180,15 +174,13 @@ 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 ")); + 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; + double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0; std::list::const_iterator it = listP.begin(); @@ -223,12 +215,9 @@ bool vpPose::coplanar(int &coplanar_plane_type) break; } P2 = *it_j; - if ((std::fabs(P2.get_oX()) <= - std::numeric_limits::epsilon()) && - (std::fabs(P2.get_oY()) <= - std::numeric_limits::epsilon()) && - (std::fabs(P2.get_oZ()) <= - std::numeric_limits::epsilon())) { + if ((std::fabs(P2.get_oX()) <= std::numeric_limits::epsilon()) && + (std::fabs(P2.get_oY()) <= std::numeric_limits::epsilon()) && + (std::fabs(P2.get_oZ()) <= std::numeric_limits::epsilon())) { not_on_origin = false; } else { not_on_origin = true; @@ -238,12 +227,9 @@ bool vpPose::coplanar(int &coplanar_plane_type) ++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::epsilon()) && - (std::fabs(P3.get_oY()) <= - std::numeric_limits::epsilon()) && - (std::fabs(P3.get_oZ()) <= - std::numeric_limits::epsilon())) { + if ((std::fabs(P3.get_oX()) <= std::numeric_limits::epsilon()) && + (std::fabs(P3.get_oY()) <= std::numeric_limits::epsilon()) && + (std::fabs(P3.get_oZ()) <= std::numeric_limits::epsilon())) { not_on_origin = false; } else { not_on_origin = true; @@ -270,8 +256,7 @@ bool vpPose::coplanar(int &coplanar_plane_type) b_c[2] = z2 - z3; cross_prod = vpColVector::crossProd(a_b, b_c); - if (cross_prod.sumSquare() <= - std::numeric_limits::epsilon()) + if (cross_prod.sumSquare() <= std::numeric_limits::epsilon()) degenerate = true; // points are collinear else degenerate = false; @@ -292,8 +277,7 @@ bool vpPose::coplanar(int &coplanar_plane_type) 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; + 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; @@ -312,8 +296,7 @@ bool vpPose::coplanar(int &coplanar_plane_type) for (it = listP.begin(); it != listP.end(); ++it) { P1 = *it; - double dist = - (a * P1.get_oX() + b * P1.get_oY() + c * P1.get_oZ() + d) / D; + double 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) { @@ -342,8 +325,7 @@ double vpPose::computeResidual(const vpHomogeneousMatrix &cMo) const { double residual_ = 0; vpPoint P; - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { P = *it; double x = P.get_x(); double y = P.get_y(); @@ -377,13 +359,11 @@ double vpPose::computeResidual(const vpHomogeneousMatrix &cMo) const - vpPose::RANSAC: Robust Ransac aproach (does't need an initialization) */ -bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, - bool (*func)(vpHomogeneousMatrix *)) +bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(vpHomogeneousMatrix *)) { if (npt < 4) { vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt); - throw(vpPoseException(vpPoseException::notEnoughPointError, - "No enough point ")); + throw(vpPoseException(vpPoseException::notEnoughPointError, "No enough point ")); } switch (method) { @@ -394,8 +374,7 @@ bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, 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 ")); + throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points ")); } // test si les point 3D sont coplanaires @@ -428,22 +407,19 @@ bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, 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 (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 ")); + 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 ")); + throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points ")); } try { poseLagrangePlan(cMo, coplanar_plane_type); @@ -456,8 +432,7 @@ bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, 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 ")); + throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points ")); } try { poseLagrangeNonPlan(cMo); @@ -472,8 +447,7 @@ bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, 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 ")); + throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points ")); } try { return poseRansac(cMo, func); @@ -521,8 +495,7 @@ bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, void vpPose::printPoint() { vpPoint P; - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { P = *it; std::cout << "3D oP " << P.oP.t(); @@ -540,8 +513,8 @@ void vpPose::printPoint() axis. If vpColor::none, red, green and blue will represent x-axiw, y-axis and z-axis respectively. */ -void vpPose::display(vpImage &I, vpHomogeneousMatrix &cMo, - vpCameraParameters &cam, double size, vpColor col) +void vpPose::display(vpImage &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, + vpColor col) { vpDisplay::displayFrame(I, cMo, cam, size, col); } @@ -555,8 +528,7 @@ void vpPose::display(vpImage &I, vpHomogeneousMatrix &cMo, axis. If vpColor::none, red, green and blue will represent x-axiw, y-axis and z-axis respectively. */ -void vpPose::display(vpImage &I, vpHomogeneousMatrix &cMo, - vpCameraParameters &cam, double size, vpColor col) +void vpPose::display(vpImage &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col) { vpDisplay::displayFrame(I, cMo, cam, size, col); } @@ -565,13 +537,11 @@ void vpPose::display(vpImage &I, vpHomogeneousMatrix &cMo, Display the coordinates of the points in the image plane that are used to compute the pose in image I. */ -void vpPose::displayModel(vpImage &I, vpCameraParameters &cam, - vpColor col) +void vpPose::displayModel(vpImage &I, vpCameraParameters &cam, vpColor col) { vpPoint P; vpImagePoint ip; - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { P = *it; vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip); vpDisplay::displayCross(I, ip, 5, col); @@ -585,13 +555,11 @@ void vpPose::displayModel(vpImage &I, vpCameraParameters &cam, Display the coordinates of the points in the image plane that are used to compute the pose in image I. */ -void vpPose::displayModel(vpImage &I, vpCameraParameters &cam, - vpColor col) +void vpPose::displayModel(vpImage &I, vpCameraParameters &cam, vpColor col) { vpPoint P; vpImagePoint ip; - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { P = *it; vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip); vpDisplay::displayCross(I, ip, 5, col); @@ -620,9 +588,7 @@ gives us the proportion of the rectangle \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, +double vpPose::poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam, vpHomogeneousMatrix &cMo) { diff --git a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp index 43651b25b9..af7839a2e5 100644 --- a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp +++ b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp @@ -68,8 +68,7 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) c3d.clear(); vpPoint P; - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::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()); @@ -137,9 +136,8 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) 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")); + throw(vpException(vpException::divideByZeroError, + "Division by zero in Dementhon pose computation: normI+normJ = 0")); } k = vpColVector::cross(I, J); @@ -147,16 +145,12 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) 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; + 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")); + throw(vpException(vpException::divideByZeroError, "Division by zero in Dementhon pose computation: no points")); } // seuil/=npt; } @@ -179,12 +173,9 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) 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]); + 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 */ @@ -213,8 +204,7 @@ static void calculRTheta(double s, double c, double &r, double &theta) } } -static void calculSolutionDementhon(double xi0, double yi0, vpColVector &I, - vpColVector &J, vpHomogeneousMatrix &cMo) +static void calculSolutionDementhon(double xi0, double yi0, vpColVector &I, vpColVector &J, vpHomogeneousMatrix &cMo) { #if (DEBUG_LEVEL1) @@ -261,8 +251,7 @@ static void calculSolutionDementhon(double xi0, double yi0, vpColVector &I, #endif } -int vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U, - vpHomogeneousMatrix &cMo) +int vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo) { #if (DEBUG_LEVEL1) @@ -279,8 +268,7 @@ int vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U, // on test si tous les points sont devant la camera for (unsigned int i = 0; i < npt; i++) { double z; - z = cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + - cMo[2][2] * c3d[i].get_oZ() + cMo[2][3]; + z = cMo[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; } @@ -298,9 +286,7 @@ int vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U, 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]; + (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++; } @@ -387,10 +373,8 @@ int vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U, k = 0; for (unsigned int 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]; + 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++; } @@ -400,10 +384,8 @@ int vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U, k = 0; for (unsigned int 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]; + 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++; } @@ -428,8 +410,7 @@ int vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U, } } #if (DEBUG_LEVEL1) - std::cout << "end vpPose::CalculArbreDementhon() return " << erreur - << std::endl; + std::cout << "end vpPose::CalculArbreDementhon() return " << erreur << std::endl; #endif return erreur; @@ -455,8 +436,7 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) vpPoint P; c3d.clear(); - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::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()); @@ -639,12 +619,9 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) 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]; + 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; @@ -670,18 +647,14 @@ double vpPose::computeResidualDementhon(const vpHomogeneousMatrix &cMo) 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 = 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()); + residual_ += vpMath::sqr(x - c3d[i].get_x()) + vpMath::sqr(y - c3d[i].get_y()); } return residual_; } diff --git a/modules/vision/src/pose-estimation/vpPoseFeatures.cpp b/modules/vision/src/pose-estimation/vpPoseFeatures.cpp index 342d3fe040..35eb36a215 100644 --- a/modules/vision/src/pose-estimation/vpPoseFeatures.cpp +++ b/modules/vision/src/pose-estimation/vpPoseFeatures.cpp @@ -43,12 +43,10 @@ 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() + : 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() { } @@ -74,8 +72,7 @@ void vpPoseFeatures::clear() delete featureVanishingPoint_Point_list[(unsigned int)i].desiredFeature; featureVanishingPoint_Point_list.clear(); - for (int i = (int)featureVanishingPoint_DuoLine_list.size() - 1; i >= 0; - i--) + for (int i = (int)featureVanishingPoint_DuoLine_list.size() - 1; i >= 0; i--) delete featureVanishingPoint_DuoLine_list[(unsigned int)i].desiredFeature; featureVanishingPoint_DuoLine_list.clear(); @@ -138,8 +135,7 @@ void vpPoseFeatures::addFeaturePoint3D(const vpPoint &p) featurePoint3D_Point_list.push_back(vpDuo()); featurePoint3D_Point_list.back().firstParam = p; featurePoint3D_Point_list.back().desiredFeature = new vpFeaturePoint3D(); - vpFeatureBuilder::create(*featurePoint3D_Point_list.back().desiredFeature, - p); + vpFeatureBuilder::create(*featurePoint3D_Point_list.back().desiredFeature, p); totalSize++; if (featurePoint3D_Point_list.size() > maxSize) @@ -154,13 +150,10 @@ void vpPoseFeatures::addFeaturePoint3D(const vpPoint &p) */ void vpPoseFeatures::addFeatureVanishingPoint(const vpPoint &p) { - featureVanishingPoint_Point_list.push_back( - vpDuo()); + featureVanishingPoint_Point_list.push_back(vpDuo()); featureVanishingPoint_Point_list.back().firstParam = p; - featureVanishingPoint_Point_list.back().desiredFeature = - new vpFeatureVanishingPoint(); - vpFeatureBuilder::create( - *featureVanishingPoint_Point_list.back().desiredFeature, p); + featureVanishingPoint_Point_list.back().desiredFeature = new vpFeatureVanishingPoint(); + vpFeatureBuilder::create(*featureVanishingPoint_Point_list.back().desiredFeature, p); totalSize++; if (featureVanishingPoint_Point_list.size() > maxSize) @@ -174,17 +167,13 @@ void vpPoseFeatures::addFeatureVanishingPoint(const vpPoint &p) \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) +void vpPoseFeatures::addFeatureVanishingPoint(const vpLine &l1, const vpLine &l2) { - featureVanishingPoint_DuoLine_list.push_back( - vpTrio()); + featureVanishingPoint_DuoLine_list.push_back(vpTrio()); 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); + featureVanishingPoint_DuoLine_list.back().desiredFeature = new vpFeatureVanishingPoint(); + vpFeatureBuilder::create(*featureVanishingPoint_DuoLine_list.back().desiredFeature, l1, l2); totalSize++; if (featureVanishingPoint_DuoLine_list.size() > maxSize) @@ -202,8 +191,7 @@ void vpPoseFeatures::addFeatureEllipse(const vpSphere &s) featureEllipse_Sphere_list.push_back(vpDuo()); featureEllipse_Sphere_list.back().firstParam = s; featureEllipse_Sphere_list.back().desiredFeature = new vpFeatureEllipse(); - vpFeatureBuilder::create(*featureEllipse_Sphere_list.back().desiredFeature, - s); + vpFeatureBuilder::create(*featureEllipse_Sphere_list.back().desiredFeature, s); totalSize++; if (featureEllipse_Sphere_list.size() > maxSize) @@ -221,8 +209,7 @@ void vpPoseFeatures::addFeatureEllipse(const vpCircle &c) featureEllipse_Circle_list.push_back(vpDuo()); featureEllipse_Circle_list.back().firstParam = c; featureEllipse_Circle_list.back().desiredFeature = new vpFeatureEllipse(); - vpFeatureBuilder::create(*featureEllipse_Circle_list.back().desiredFeature, - c); + vpFeatureBuilder::create(*featureEllipse_Circle_list.back().desiredFeature, c); totalSize++; if (featureEllipse_Circle_list.size() > maxSize) @@ -257,13 +244,11 @@ void vpPoseFeatures::addFeatureLine(const vpLine &l) */ void vpPoseFeatures::addFeatureLine(const vpCylinder &c, const int &line) { - featureLine_DuoLineInt_List.push_back( - vpTrio()); + featureLine_DuoLineInt_List.push_back(vpTrio()); 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); + vpFeatureBuilder::create(*featureLine_DuoLineInt_List.back().desiredFeature, c, line); totalSize++; if (featureLine_DuoLineInt_List.size() > maxSize) @@ -279,14 +264,11 @@ void vpPoseFeatures::addFeatureLine(const vpCylinder &c, const int &line) */ void vpPoseFeatures::addFeatureSegment(vpPoint &P1, vpPoint &P2) { - featureSegment_DuoPoints_list.push_back( - vpTrio()); + featureSegment_DuoPoints_list.push_back(vpTrio()); 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); + featureSegment_DuoPoints_list.back().desiredFeature = new vpFeatureSegment(); + vpFeatureBuilder::create(*featureSegment_DuoPoints_list.back().desiredFeature, P1, P2); totalSize++; if (featureSegment_DuoPoints_list.size() > maxSize) @@ -300,8 +282,7 @@ void vpPoseFeatures::addFeatureSegment(vpPoint &P1, vpPoint &P2) \param err : Resulting error vector. \param L : Resulting interaction matrix. */ -void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix &cMo, - vpColVector &err, vpMatrix &L) +void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix &cMo, vpColVector &err, vpMatrix &L) { err = vpColVector(); L = vpMatrix(); @@ -336,8 +317,7 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix &cMo, vpPoint p(featureVanishingPoint_Point_list[i].firstParam); p.track(cMo); vpFeatureBuilder::create(fvp, p); - err.stack( - fvp.error(*(featureVanishingPoint_Point_list[i].desiredFeature))); + err.stack(fvp.error(*(featureVanishingPoint_Point_list[i].desiredFeature))); L.stack(fvp.interaction()); } // From Duo of vpLines @@ -348,8 +328,7 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix &cMo, l1.track(cMo); l2.track(cMo); vpFeatureBuilder::create(fvp, l1, l2); - err.stack( - fvp.error(*(featureVanishingPoint_DuoLine_list[i].desiredFeature))); + err.stack(fvp.error(*(featureVanishingPoint_DuoLine_list[i].desiredFeature))); L.stack(fvp.interaction()); } @@ -388,8 +367,7 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix &cMo, vpFeatureLine fl; vpCylinder c(featureLine_DuoLineInt_List[i].firstParam); c.track(cMo); - vpFeatureBuilder::create(fl, c, - featureLine_DuoLineInt_List[i].secondParam); + vpFeatureBuilder::create(fl, c, featureLine_DuoLineInt_List[i].secondParam); err.stack(fl.error(*(featureLine_DuoLineInt_List[i].desiredFeature))); L.stack(fl.interaction()); } @@ -432,8 +410,7 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix &cMo, \cite Comport06b. */ -void vpPoseFeatures::computePose(vpHomogeneousMatrix &cMo, - const vpPoseFeaturesMethodType &type) +void vpPoseFeatures::computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type) { switch (type) { case VIRTUAL_VS: @@ -473,8 +450,7 @@ void vpPoseFeatures::computePoseVVS(vpHomogeneousMatrix &cMo) unsigned int iter = 0; // while((int)((residu_1 - r)*1e12) != 0 ) - while (std::fabs((residu_1 - r) * 1e12) > - std::numeric_limits::epsilon()) { + while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits::epsilon()) { residu_1 = r; // Compute the interaction matrix and the error @@ -505,8 +481,7 @@ void vpPoseFeatures::computePoseVVS(vpHomogeneousMatrix &cMo) } if (computeCovariance) - covarianceMatrix = - vpMatrix::computeCovarianceMatrix(L, v, -lambda * err); + covarianceMatrix = vpMatrix::computeCovarianceMatrix(L, v, -lambda * err); } catch (...) { vpERROR_TRACE("vpPoseFeatures::computePoseVVS"); @@ -538,8 +513,7 @@ void vpPoseFeatures::computePoseRobustVVS(vpHomogeneousMatrix &cMo) unsigned int iter = 0; // while((int)((residu_1 - r)*1e12) !=0) - while (std::fabs((residu_1 - r) * 1e12) > - std::numeric_limits::epsilon()) { + while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits::epsilon()) { residu_1 = r; // Compute the interaction matrix and the error @@ -591,10 +565,10 @@ void vpPoseFeatures::computePoseRobustVVS(vpHomogeneousMatrix &cMo) } 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. + 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/modules/vision/src/pose-estimation/vpPoseLagrange.cpp b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp index 664e1a0ec3..b52ec0f1c7 100644 --- a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp +++ b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp @@ -49,8 +49,7 @@ /* rotation */ /**********************************************************************/ -static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, - unsigned int nc1, unsigned int nc3, +static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1, unsigned int nc3, vpColVector &x1, vpColVector &x2) { @@ -130,8 +129,7 @@ static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, //#define EPS 1.e-5 -static void lagrange(vpMatrix &a, vpMatrix &b, vpColVector &x1, - vpColVector &x2) +static void lagrange(vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2) { #if (DEBUG_LEVEL1) std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl; @@ -259,8 +257,7 @@ static void lagrange(vpMatrix &a, vpMatrix &b, vpColVector &x1, 3: if plane z=cst 0: any other plane */ -void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, - const int coplanar_plane_type) +void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type) { #if (DEBUG_LEVEL1) @@ -279,8 +276,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, i = 0; if (coplanar_plane_type == 1) { // plane ax=d - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { P = *it; a[k][0] = -P.get_oY(); a[k][1] = 0.0; @@ -308,8 +304,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, } } else if (coplanar_plane_type == 2) { // plane by=d - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { P = *it; a[k][0] = -P.get_oX(); a[k][1] = 0.0; @@ -338,8 +333,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, } else { // plane cz=d or any other - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { P = *it; a[k][0] = -P.get_oX(); a[k][1] = 0.0; @@ -380,8 +374,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, #if (DEBUG_LEVEL2) { - std::cout << "ax1+bx2 (devrait etre 0) " << (a * X1 + b * X2).t() - << std::endl; + std::cout << "ax1+bx2 (devrait etre 0) " << (a * X1 + b * X2).t() << std::endl; std::cout << "norme X1 " << X1.sumSquare() << std::endl; ; } @@ -403,8 +396,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, // 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 + 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; @@ -416,9 +408,8 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, // << (*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)")); + throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation " + "(planar plane case)")); } s = 1.0 / sqrt(s); @@ -496,8 +487,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) vpPoint P; i = 0; - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { P = *it; a[k][0] = -P.get_oX(); a[k][1] = 0.0; @@ -553,8 +543,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) #if (DEBUG_LEVEL2) { - std::cout << "ax1+bx2 (devrait etre 0) " << (a * X1 + b * X2).t() - << std::endl; + std::cout << "ax1+bx2 (devrait etre 0) " << (a * X1 + b * X2).t() << std::endl; std::cout << "norme X1 " << X1.sumSquare() << std::endl; ; } @@ -574,8 +563,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) // 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 + 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; @@ -588,9 +576,8 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) // (*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)")); + throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation (non " + "planar plane case)")); } s = 1.0 / sqrt(s); diff --git a/modules/vision/src/pose-estimation/vpPoseLowe.cpp b/modules/vision/src/pose-estimation/vpPoseLowe.cpp index 46d5127379..0fc22852a4 100644 --- a/modules/vision/src/pose-estimation/vpPoseLowe.cpp +++ b/modules/vision/src/pose-estimation/vpPoseLowe.cpp @@ -89,8 +89,7 @@ static double XO[NBPTMAX], YO[NBPTMAX], ZO[NBPTMAX]; #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 fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag); void eval_function(int npt, double *xc, double *f) { @@ -139,8 +138,7 @@ void eval_function(int npt, double *xc, double *f) * 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) +void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag) { double u[X3_SIZE]; // rd[X3_SIZE][X3_SIZE], vpRotationMatrix rd; @@ -162,8 +160,7 @@ void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, /* a partir de l'axe de rotation, calcul de la matrice de rotation. */ // rot_mat(u, rd); - double tt = - sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]); /* angle de rot */ + double 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 */ @@ -187,20 +184,17 @@ void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, /* derive des fonctions rx, ry et rz par rapport * a tt, u1, u2, u3. */ - double drxt = (si * u1 * u3 + co * u2) * z + - (si * u1 * u2 - co * u3) * y + (si * u1 * u1 - si) * x; + double drxt = (si * u1 * u3 + co * u2) * z + (si * u1 * u2 - co * u3) * y + (si * u1 * u1 - si) * x; double drxu1 = mco * u3 * z + mco * u2 * y + 2 * mco * u1 * x; double drxu2 = si * z + mco * u1 * y; double drxu3 = mco * u1 * z - si * y; - double dryt = (si * u2 * u3 - co * u1) * z + (si * u2 * u2 - si) * y + - (co * u3 + si * u1 * u2) * x; + double dryt = (si * u2 * u3 - co * u1) * z + (si * u2 * u2 - si) * y + (co * u3 + si * u1 * u2) * x; double dryu1 = mco * u2 * x - si * z; double dryu2 = mco * u3 * z + 2 * mco * u2 * y + mco * u1 * x; double dryu3 = mco * u2 * z + si * x; - double drzt = (si * u3 * u3 - si) * z + (si * u2 * u3 + co * u1) * y + - (si * u1 * u3 - co * u2) * x; + double drzt = (si * u3 * u3 - si) * z + (si * u2 * u3 + co * u1) * y + (si * u1 * u3 - co * u2) * x; double drzu1 = si * y + mco * u3 * x; double drzu2 = mco * u3 * y - si * x; double drzu3 = 2 * mco * u3 * z + mco * u2 * y + mco * u1 * x; @@ -229,15 +223,10 @@ void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, *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; + *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; @@ -248,14 +237,11 @@ void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, *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; + 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; @@ -288,10 +274,10 @@ void vpPose::poseLowe(vpHomogeneousMatrix &cMo) // 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 */ + n = NBR_PAR; /* nombres d'inconnues */ + m = (int)(2 * npt); /* nombres d'equations */ + lwa = 2 * NBPTMAX + 50; /* taille du vecteur de travail */ + ldfjac = 2 * NBPTMAX; /* nombre d'elements max sur une ligne */ tol = std::numeric_limits::epsilon(); /* critere d'arret */ // c = cam ; @@ -308,8 +294,7 @@ void vpPose::poseLowe(vpHomogeneousMatrix &cMo) vpPoint P; unsigned int i_ = 0; - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::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 ; @@ -318,8 +303,7 @@ void vpPose::poseLowe(vpHomogeneousMatrix &cMo) ZO[i_] = P.get_oZ(); ++i_; } - tst_lmder = lmder1(&fcn, m, n, sol, f, &jac[0][0], ldfjac, tol, &info, ipvt, - lwa, wa); + 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"; diff --git a/modules/vision/src/pose-estimation/vpPoseRansac.cpp b/modules/vision/src/pose-estimation/vpPoseRansac.cpp index 1e7d904785..ea0f0bed6f 100644 --- a/modules/vision/src/pose-estimation/vpPoseRansac.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRansac.cpp @@ -182,11 +182,9 @@ struct FindDegeneratePoint { bool operator()(const vpPoint &pt) { - return ((std::fabs(m_pt.oP[0] - pt.oP[0]) < eps && - std::fabs(m_pt.oP[1] - pt.oP[1]) < eps && + return ((std::fabs(m_pt.oP[0] - pt.oP[0]) < eps && std::fabs(m_pt.oP[1] - pt.oP[1]) < eps && std::fabs(m_pt.oP[2] - pt.oP[2]) < eps) || - (std::fabs(m_pt.p[0] - pt.p[0]) < eps && - std::fabs(m_pt.p[1] - pt.p[1]) < eps)); + (std::fabs(m_pt.p[0] - pt.p[0]) < eps && std::fabs(m_pt.p[1] - pt.p[1]) < eps)); } vpPoint m_pt; @@ -215,16 +213,11 @@ struct HashDuplicate { struct ComparePointDuplicateUnorderedMap { bool operator()(const vpPoint &point1, const vpPoint &point2) const { - return (std::fabs(point1.oP[0] - point2.oP[0]) < - std::numeric_limits::epsilon() && - std::fabs(point1.oP[1] - point2.oP[1]) < - std::numeric_limits::epsilon() && - std::fabs(point1.oP[2] - point2.oP[2]) < - std::numeric_limits::epsilon() && - std::fabs(point1.p[0] - point2.p[0]) < - std::numeric_limits::epsilon() && - std::fabs(point1.p[1] - point2.p[1]) < - std::numeric_limits::epsilon()); + return (std::fabs(point1.oP[0] - point2.oP[0]) < std::numeric_limits::epsilon() && + std::fabs(point1.oP[1] - point2.oP[1]) < std::numeric_limits::epsilon() && + std::fabs(point1.oP[2] - point2.oP[2]) < std::numeric_limits::epsilon() && + std::fabs(point1.p[0] - point2.p[0]) < std::numeric_limits::epsilon() && + std::fabs(point1.p[1] - point2.p[1]) < std::numeric_limits::epsilon()); } }; #endif @@ -243,8 +236,7 @@ bool vpPose::RansacFunctor::poseRansacImpl() vpPoint p; // Point used to project using the estimated pose bool foundSolution = false; - while (nbTrials < m_ransacMaxTrials && - m_nbInliers < (unsigned int)m_ransacNbInlierConsensus) { + while (nbTrials < m_ransacMaxTrials && m_nbInliers < (unsigned int)m_ransacNbInlierConsensus) { // Hold the list of the index of the inliers (points in the consensus set) std::vector cur_consensus; // Hold the list of the index of the outliers @@ -268,8 +260,7 @@ bool vpPose::RansacFunctor::poseRansacImpl() vpPose poseMin; for (unsigned int i = 0; i < nbMinRandom;) { - if ((size_t)std::count(usedPt.begin(), usedPt.end(), true) == - usedPt.size()) { + 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; @@ -296,9 +287,8 @@ bool vpPose::RansacFunctor::poseRansacImpl() bool degenerate = false; if (m_checkDegeneratePoints) { - if (std::find_if( - poseMin.listOfPoints.begin(), poseMin.listOfPoints.end(), - FindDegeneratePoint(pt)) != poseMin.listOfPoints.end()) { + if (std::find_if(poseMin.listOfPoints.begin(), poseMin.listOfPoints.end(), FindDegeneratePoint(pt)) != + poseMin.listOfPoints.end()) { degenerate = true; } } @@ -378,21 +368,17 @@ bool vpPose::RansacFunctor::poseRansacImpl() if (isPoseValid && r < m_ransacThreshold) { unsigned int nbInliersCur = 0; unsigned int iter = 0; - for (std::vector::const_iterator - it = m_listOfUniquePoints.begin(); - it != m_listOfUniquePoints.end(); ++it, iter++) { + for (std::vector::const_iterator it = m_listOfUniquePoints.begin(); it != m_listOfUniquePoints.end(); + ++it, iter++) { p.setWorldCoordinates(it->get_oX(), it->get_oY(), it->get_oZ()); p.track(m_cMo); - double d = vpMath::sqr(p.get_x() - it->get_x()) + - vpMath::sqr(p.get_y() - it->get_y()); + double d = vpMath::sqr(p.get_x() - it->get_x()) + vpMath::sqr(p.get_y() - it->get_y()); double error = sqrt(d); if (error < m_ransacThreshold) { bool degenerate = false; if (m_checkDegeneratePoints) { - if (std::find_if(cur_inliers.begin(), cur_inliers.end(), - FindDegeneratePoint(*it)) != - cur_inliers.end()) { + if (std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(*it)) != cur_inliers.end()) { degenerate = true; } } @@ -451,8 +437,7 @@ vpThread::Return vpPose::poseRansacImplThread(vpThread::Args arg) otherwise \return True if we found at least 4 points with a reprojection error below ransacThreshold. */ -bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, - bool (*func)(vpHomogeneousMatrix *)) +bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(vpHomogeneousMatrix *)) { // Check only for adding / removing problem // Do not take into account problem with element modification here @@ -471,33 +456,26 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, if (listOfPoints.size() < 4) { // vpERROR_TRACE("Not enough point to compute the pose"); - throw(vpPoseException(vpPoseException::notInitializedError, - "Not enough point to compute the pose")); + throw(vpPoseException(vpPoseException::notInitializedError, "Not enough point to compute the pose")); } std::vector listOfUniquePoints; std::map mapOfUniquePointIndex; // Get RANSAC flags - bool prefilterDuplicatePoints = - (ransacFlags & PREFILTER_DUPLICATE_POINTS) != 0; - bool prefilterAlmostDuplicatePoints = - (ransacFlags & PREFILTER_ALMOST_DUPLICATE_POINTS) != 0; - bool prefilterDegeneratePoints = - (ransacFlags & PREFILTER_DEGENERATE_POINTS) != 0; + bool prefilterDuplicatePoints = (ransacFlags & PREFILTER_DUPLICATE_POINTS) != 0; + bool prefilterAlmostDuplicatePoints = (ransacFlags & PREFILTER_ALMOST_DUPLICATE_POINTS) != 0; + bool prefilterDegeneratePoints = (ransacFlags & PREFILTER_DEGENERATE_POINTS) != 0; bool checkDegeneratePoints = (ransacFlags & CHECK_DEGENERATE_POINTS) != 0; - if (prefilterDuplicatePoints || prefilterAlmostDuplicatePoints || - prefilterDegeneratePoints) { + if (prefilterDuplicatePoints || prefilterAlmostDuplicatePoints || prefilterDegeneratePoints) { // Prefiltering if (prefilterDuplicatePoints) { #if defined(VISP_HAVE_CPP11_COMPATIBILITY) - std::unordered_map - filterMap; + std::unordered_map filterMap; size_t index_pt = 0; - for (std::vector::const_iterator it_pt = listOfPoints.begin(); - it_pt != listOfPoints.end(); ++it_pt, index_pt++) { + for (std::vector::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); + ++it_pt, index_pt++) { if (filterMap.find(*it_pt) == filterMap.end()) { filterMap[*it_pt] = index_pt; @@ -508,8 +486,8 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, #else std::map filterMap; size_t index_pt = 0; - for (std::vector::const_iterator it_pt = listOfPoints.begin(); - it_pt != listOfPoints.end(); ++it_pt, index_pt++) { + for (std::vector::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); + ++it_pt, index_pt++) { if (filterMap.find(*it_pt) == filterMap.end()) { filterMap[*it_pt] = index_pt; @@ -521,8 +499,8 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, } else if (prefilterAlmostDuplicatePoints) { std::map filterMap; size_t index_pt = 0; - for (std::vector::const_iterator it_pt = listOfPoints.begin(); - it_pt != listOfPoints.end(); ++it_pt, index_pt++) { + for (std::vector::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); + ++it_pt, index_pt++) { if (filterMap.find(*it_pt) == filterMap.end()) { filterMap[*it_pt] = index_pt; @@ -532,23 +510,19 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, } } else { // Remove other degenerate object points - std::map - filterObjectPointMap; + std::map filterObjectPointMap; size_t index_pt = 0; - for (std::vector::const_iterator it_pt = listOfPoints.begin(); - it_pt != listOfPoints.end(); ++it_pt, index_pt++) { + for (std::vector::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); + ++it_pt, index_pt++) { if (filterObjectPointMap.find(*it_pt) == filterObjectPointMap.end()) { filterObjectPointMap[*it_pt] = index_pt; } } - std::map - filterImagePointMap; - for (std::map::const_iterator it = - filterObjectPointMap.begin(); + std::map filterImagePointMap; + for (std::map::const_iterator it = filterObjectPointMap.begin(); it != filterObjectPointMap.end(); ++it) { - if (filterImagePointMap.find(it->first) == - filterImagePointMap.end()) { + if (filterImagePointMap.find(it->first) == filterImagePointMap.end()) { filterImagePointMap[it->first] = it->second; listOfUniquePoints.push_back(it->first); @@ -561,16 +535,15 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, listOfUniquePoints = listOfPoints; size_t index_pt = 0; - for (std::vector::const_iterator it_pt = listOfPoints.begin(); - it_pt != listOfPoints.end(); ++it_pt, index_pt++) { + for (std::vector::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); + ++it_pt, index_pt++) { mapOfUniquePointIndex[index_pt] = index_pt; } } unsigned int size = (unsigned int)listOfUniquePoints.size(); if (size < 4) { - throw(vpPoseException(vpPoseException::notInitializedError, - "Not enough point to compute the pose")); + throw(vpPoseException(vpPoseException::notInitializedError, "Not enough point to compute the pose")); } bool executeParallelVersion = useParallelRansac; @@ -630,8 +603,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, #if defined(VISP_HAVE_OPENMP) // Set different seeds for each thread in the team, rand() is not thread // safe - unsigned int initial_seed = - (unsigned int)omp_get_thread_num(); // same seed each time + unsigned int initial_seed = (unsigned int)omp_get_thread_num(); // same seed each time //(unsigned int) (int(time(NULL)) ^ omp_get_thread_num()); if (omp_get_num_threads() == 1) { initial_seed = 0; // Same seed at each run @@ -680,8 +652,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, vpPose poseMin; for (unsigned int i = 0; i < nbMinRandom;) { - if ((size_t)std::count(usedPt.begin(), usedPt.end(), true) == - usedPt.size()) { + 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; @@ -708,9 +679,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool degenerate = false; if (checkDegeneratePoints) { - if (std::find_if(poseMin.listOfPoints.begin(), - poseMin.listOfPoints.end(), - FindDegeneratePoint(pt)) != + if (std::find_if(poseMin.listOfPoints.begin(), poseMin.listOfPoints.end(), FindDegeneratePoint(pt)) != poseMin.listOfPoints.end()) { degenerate = true; } @@ -781,21 +750,17 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, if (isPoseValid && r < ransacThreshold) { unsigned int nbInliersCur = 0; unsigned int iter = 0; - for (std::vector::const_iterator - it = listOfUniquePoints.begin(); + for (std::vector::const_iterator it = listOfUniquePoints.begin(); it != listOfUniquePoints.end(); ++it, iter++) { - p.setWorldCoordinates(it->get_oX(), it->get_oY(), - it->get_oZ()); + p.setWorldCoordinates(it->get_oX(), it->get_oY(), it->get_oZ()); p.track(cMo_tmp); - double d = vpMath::sqr(p.get_x() - it->get_x()) + - vpMath::sqr(p.get_y() - it->get_y()); + double d = vpMath::sqr(p.get_x() - it->get_x()) + vpMath::sqr(p.get_y() - it->get_y()); double error = sqrt(d); if (error < ransacThreshold) { bool degenerate = false; if (checkDegeneratePoints) { - if (std::find_if(cur_inliers.begin(), cur_inliers.end(), - FindDegeneratePoint(*it)) != + if (std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(*it)) != cur_inliers.end()) { degenerate = true; } @@ -842,28 +807,22 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, int splitTrials = ransacMaxTrials / nbThreads; for (size_t i = 0; i < (size_t)nbThreads; i++) { - unsigned int initial_seed = - (unsigned int)i; //((unsigned int) time(NULL) ^ i); + unsigned int initial_seed = (unsigned int)i; //((unsigned int) time(NULL) ^ i); if (i < (size_t)nbThreads - 1) { - ransac_func[i] = RansacFunctor( - cMo, ransacNbInlierConsensus, splitTrials, ransacThreshold, - initial_seed, checkDegeneratePoints, listOfUniquePoints, func); + ransac_func[i] = RansacFunctor(cMo, ransacNbInlierConsensus, splitTrials, ransacThreshold, initial_seed, + checkDegeneratePoints, listOfUniquePoints, func); } else { - int maxTrialsRemainder = - ransacMaxTrials - splitTrials * (nbThreads - 1); - ransac_func[i] = RansacFunctor( - cMo, ransacNbInlierConsensus, maxTrialsRemainder, ransacThreshold, - initial_seed, checkDegeneratePoints, listOfUniquePoints, func); + int maxTrialsRemainder = ransacMaxTrials - splitTrials * (nbThreads - 1); + ransac_func[i] = RansacFunctor(cMo, ransacNbInlierConsensus, maxTrialsRemainder, ransacThreshold, initial_seed, + checkDegeneratePoints, listOfUniquePoints, func); } - threads[(size_t)i] = new vpThread((vpThread::Fn)poseRansacImplThread, - (vpThread::Args)&ransac_func[i]); + threads[(size_t)i] = new vpThread((vpThread::Fn)poseRansacImplThread, (vpThread::Args)&ransac_func[i]); } // Get the best pose between the threads vpPose final_pose; - for (std::vector::const_iterator it = listOfPoints.begin(); - it != listOfPoints.end(); ++it) { + for (std::vector::const_iterator it = listOfPoints.begin(); it != listOfPoints.end(); ++it) { final_pose.addPoint(*it); } @@ -890,9 +849,8 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, #endif } else { // Sequential RANSAC - RansacFunctor sequentialRansac( - cMo, ransacNbInlierConsensus, ransacMaxTrials, ransacThreshold, 0, - checkDegeneratePoints, listOfUniquePoints, func); + RansacFunctor sequentialRansac(cMo, ransacNbInlierConsensus, ransacMaxTrials, ransacThreshold, 0, + checkDegeneratePoints, listOfUniquePoints, func); sequentialRansac(); foundSolution = sequentialRansac.getResult(); @@ -927,8 +885,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, // 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) + if (nbInliers >= nbMinRandom) // if(nbInliers >= (unsigned)ransacNbInlierConsensus) { // Refine the solution using all the points in the consensus set and // with VVS pose estimation @@ -941,11 +898,9 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, } // Update the list of inlier index - for (std::vector::const_iterator it_index = - best_consensus.begin(); + for (std::vector::const_iterator it_index = best_consensus.begin(); it_index != best_consensus.end(); ++it_index) { - ransacInlierIndex.push_back( - (unsigned int)mapOfUniquePointIndex[*it_index]); + ransacInlierIndex.push_back((unsigned int)mapOfUniquePointIndex[*it_index]); } // Flags set if pose computation is OK @@ -1026,8 +981,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, or \p maxIterations if it exceeds the desired upper bound or \e INT_MAX if maxIterations=-1. */ -int vpPose::computeRansacIterations(double probability, double epsilon, - const int sampleSize, int maxIterations) +int vpPose::computeRansacIterations(double probability, double epsilon, const int sampleSize, int maxIterations) { probability = (std::max)(probability, 0.0); probability = (std::min)(probability, 1.0); @@ -1057,9 +1011,7 @@ int vpPose::computeRansacIterations(double probability, double epsilon, return 0; } - N = log((std::max)(1.0 - probability, - std::numeric_limits::epsilon())) / - logval; + N = log((std::max)(1.0 - probability, std::numeric_limits::epsilon())) / logval; if (logval < 0.0 && N < maxIterations) { return (int)ceil(N); } @@ -1091,10 +1043,9 @@ int vpPose::computeRansacIterations(double probability, double epsilon, numberOfInlierToReachAConsensus and \e threshold cannot be found. */ void vpPose::findMatch(std::vector &p2D, std::vector &p3D, - const unsigned int &numberOfInlierToReachAConsensus, - const double &threshold, unsigned int &ninliers, - std::vector &listInliers, - vpHomogeneousMatrix &cMo, const int &maxNbTrials) + const unsigned int &numberOfInlierToReachAConsensus, const double &threshold, + unsigned int &ninliers, std::vector &listInliers, vpHomogeneousMatrix &cMo, + const int &maxNbTrials) { vpPose pose; @@ -1112,12 +1063,9 @@ void vpPose::findMatch(std::vector &p2D, std::vector &p3D, if (pose.listP.size() < 4) { vpERROR_TRACE("Ransac method cannot be used in that case "); vpERROR_TRACE("(at least 4 points are required)"); - vpERROR_TRACE("Not enough point (%d) to compute the pose ", - pose.listP.size()); - throw( - vpPoseException(vpPoseException::notEnoughPointError, - "Not enough point (%d) to compute the pose by ransac", - pose.listP.size())); + vpERROR_TRACE("Not enough point (%d) to compute the pose ", pose.listP.size()); + throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough point (%d) to compute the pose by ransac", + pose.listP.size())); } else { pose.setRansacMaxTrials(maxNbTrials); pose.setRansacNbInliersToReachConsensus(numberOfInlierToReachAConsensus); diff --git a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp index f6e3f0745b..48d1d58973 100644 --- a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp +++ b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp @@ -75,8 +75,7 @@ void vpPose::poseVirtualVS(vpHomogeneousMatrix &cMo) // create sd unsigned int k = 0; - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { P = *it; sd[2 * k] = P.get_x(); sd[2 * k + 1] = P.get_y(); @@ -93,8 +92,7 @@ void vpPose::poseVirtualVS(vpHomogeneousMatrix &cMo) // Compute the interaction matrix and the error k = 0; - for (std::list::const_iterator it = lP.begin(); it != lP.end(); - ++it) { + for (std::list::const_iterator it = lP.begin(); it != lP.end(); ++it) { P = *it; // forward projection of the 3D model for a given pose // change frame coordinates @@ -143,8 +141,7 @@ void vpPose::poseVirtualVS(vpHomogeneousMatrix &cMo) } if (computeCovariance) - covarianceMatrix = - vpMatrix::computeCovarianceMatrixVVS(cMoPrev, err, L); + covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, err, L); } catch (...) { @@ -185,8 +182,7 @@ void vpPose::poseVirtualVSrobust(vpHomogeneousMatrix &cMo) // create sd unsigned int k_ = 0; - for (std::list::const_iterator it = listP.begin(); - it != listP.end(); ++it) { + for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { P = *it; sd[2 * k_] = P.get_x(); sd[2 * k_ + 1] = P.get_y(); @@ -200,14 +196,12 @@ void vpPose::poseVirtualVSrobust(vpHomogeneousMatrix &cMo) w = 1; // while((int)((residu_1 - r)*1e12) !=0) - while (std::fabs((residu_1 - r) * 1e12) > - std::numeric_limits::epsilon()) { + while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits::epsilon()) { residu_1 = r; // Compute the interaction matrix and the error k_ = 0; - for (std::list::const_iterator it = lP.begin(); it != lP.end(); - ++it) { + for (std::list::const_iterator it = lP.begin(); it != lP.end(); ++it) { P = *it; // forward projection of the 3D model for a given pose // change frame coordinates @@ -263,10 +257,10 @@ void vpPose::poseVirtualVSrobust(vpHomogeneousMatrix &cMo) } 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. + 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 index 74118efcb1..1d2b8856d0 100644 --- a/modules/vision/test/homography/testDisplacement.cpp +++ b/modules/vision/test/homography/testDisplacement.cpp @@ -53,20 +53,17 @@ #include #include -bool test(const std::string &s, const vpHomography &H, - const std::vector &bench) +bool test(const std::string &s, const vpHomography &H, const std::vector &bench) { static unsigned int cpt = 0; std::cout << "** Test " << ++cpt << std::endl; - std::cout << s << "(" << H.getRows() << "," << H.getCols() << ") = \n[" << H - << "]" << 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::epsilon()) { + if (std::fabs(H.data[i] - bench[i]) > std::fabs(H.data[i]) * std::numeric_limits::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return false; } @@ -118,8 +115,7 @@ int main() std::cout << "T" << std::endl << T.t() << std::endl; std::cout << "n" << std::endl << n.t() << std::endl; } - std::cout << "------------------------------------------------------" - << std::endl; + std::cout << "------------------------------------------------------" << std::endl; { vpThetaUVector tu(vpMath::rad(90), vpMath::rad(120), vpMath::rad(45)); @@ -155,8 +151,7 @@ int main() std::cout << "n" << std::endl << n.t() << std::endl; } - std::cout << "------------------------------------------------------" - << std::endl; + std::cout << "------------------------------------------------------" << std::endl; { vpThetaUVector tu(vpMath::rad(-190), vpMath::rad(12), vpMath::rad(-45)); diff --git a/modules/vision/test/key-point/testKeyPoint-2.cpp b/modules/vision/test/key-point/testKeyPoint-2.cpp index 9f07c8adbb..170a08f75a 100644 --- a/modules/vision/test/key-point/testKeyPoint-2.cpp +++ b/modules/vision/test/key-point/testKeyPoint-2.cpp @@ -58,8 +58,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -105,8 +104,7 @@ OPTIONS: \n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -176,11 +174,9 @@ int main(int argc, const char **argv) std::string dirname = vpIoTools::createFilePath(env_ipath, "mbt/cube"); // Build the name of the image files - std::string filenameRef = - vpIoTools::createFilePath(dirname, "image0000.pgm"); + std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm"); vpImageIo::read(I, filenameRef); - std::string filenameCur = - vpIoTools::createFilePath(dirname, "image%04d.pgm"); + std::string filenameCur = vpIoTools::createFilePath(dirname, "image%04d.pgm"); #if defined VISP_HAVE_X11 vpDisplayX display; @@ -200,8 +196,7 @@ int main(int argc, const char **argv) vpCameraParameters cam; vpMbEdgeTracker tracker; // Load config for tracker - std::string tracker_config_file = - vpIoTools::createFilePath(env_ipath, "mbt/cube.xml"); + std::string tracker_config_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml"); #ifdef VISP_HAVE_XML2 tracker.loadConfigFile(tracker_config_file); @@ -217,8 +212,7 @@ int main(int argc, const char **argv) me.setSampleStep(4); me.setNbTotalSample(250); tracker.setMovingEdge(me); - cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, - 234.5083345); + cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, 234.5083345); tracker.setCameraParameters(cam); tracker.setNearClippingDistance(0.01); tracker.setFarClippingDistance(100.0); @@ -229,18 +223,15 @@ int main(int argc, const char **argv) tracker.setAngleDisappear(vpMath::rad(89)); // Load CAO model - std::string cao_model_file = - vpIoTools::createFilePath(env_ipath, "mbt/cube.cao"); + std::string cao_model_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.cao"); tracker.loadModel(cao_model_file); // Initialize the pose - std::string init_file = - vpIoTools::createFilePath(env_ipath, "mbt/cube.init"); + std::string init_file = vpIoTools::createFilePath(env_ipath, "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); + vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); tracker.initFromPose(I, cMoi); } @@ -257,8 +248,7 @@ int main(int argc, const char **argv) #if (VISP_HAVE_OPENCV_VERSION < 0x030000) keypoints.setDetectorParameter("ORB", "nLevels", 1); #else - cv::Ptr orb_detector = - keypoints.getDetector("ORB").dynamicCast(); + cv::Ptr orb_detector = keypoints.getDetector("ORB").dynamicCast(); if (orb_detector != NULL) { orb_detector->setNLevels(1); } @@ -273,15 +263,14 @@ int main(int argc, const char **argv) // Keep only keypoints on the cube std::vector polygons; std::vector > roisPt; - std::pair, std::vector > > - pair = tracker.getPolygonFaces(true); // To detect an issue with CI + std::pair, std::vector > > pair = + tracker.getPolygonFaces(true); // To detect an issue with CI polygons = pair.first; roisPt = pair.second; // Compute the 3D coordinates std::vector points3f; - vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, - polygons, roisPt, points3f); + vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f); // Build the reference keypoints keypoints.buildReference(I, trainKeyPoints, points3f, false, 1); @@ -291,8 +280,7 @@ int main(int argc, const char **argv) vpImageIo::read(I, filenameRef); // Init pose at image 150 - cMo.buildFrom(0.02651282185, -0.03713587374, 0.6873765919, 2.314744454, - 0.3492296488, -0.1226054828); + cMo.buildFrom(0.02651282185, -0.03713587374, 0.6873765919, 2.314744454, 0.3492296488, -0.1226054828); tracker.initFromPose(I, cMo); // Detect keypoints on the image 150 @@ -305,8 +293,7 @@ int main(int argc, const char **argv) roisPt = pair.second; // Compute the 3D coordinates - vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, - polygons, roisPt, points3f); + vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f); // Build the reference keypoints keypoints.buildReference(I, trainKeyPoints, points3f, true, 2); @@ -316,8 +303,7 @@ int main(int argc, const char **argv) vpImageIo::read(I, filenameRef); // Init pose at image 200 - cMo.buildFrom(0.02965448956, -0.07283091786, 0.7253526051, 2.300529617, - -0.4286674806, 0.1788761025); + cMo.buildFrom(0.02965448956, -0.07283091786, 0.7253526051, 2.300529617, -0.4286674806, 0.1788761025); tracker.initFromPose(I, cMo); // Detect keypoints on the image 200 @@ -329,8 +315,7 @@ int main(int argc, const char **argv) roisPt = pair.second; // Compute the 3D coordinates - vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, - polygons, roisPt, points3f); + vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f); // Build the reference keypoints keypoints.buildReference(I, trainKeyPoints, points3f, true, 3); @@ -357,17 +342,13 @@ int main(int argc, const char **argv) if (opt_display) { display2.setDownScalingFactor(vpDisplay::SCALE_AUTO); - display2.init(IMatching, 0, - (int)I.getHeight() / vpDisplay::getDownScalingFactor(I) + - 80, - "IMatching"); + display2.init(IMatching, 0, (int)I.getHeight() / vpDisplay::getDownScalingFactor(I) + 80, "IMatching"); } bool opt_click = false; double error; vpMouseButton::vpMouseButtonType button; - while ((opt_display && !g.end()) || - (!opt_display && g.getFrameIndex() < 30)) { + while ((opt_display && !g.end()) || (!opt_display && g.getFrameIndex() < 30)) { g.acquire(I); if (opt_display) { @@ -388,14 +369,10 @@ int main(int argc, const char **argv) tracker.display(I, cMo, cam, vpColor::red, 2); vpDisplay::displayFrame(I, cMo, cam, 0.025, vpColor::none, 3); - std::vector ransacInliers = - keypoints.getRansacInliers(); - std::vector ransacOutliers = - keypoints.getRansacOutliers(); + std::vector ransacInliers = keypoints.getRansacInliers(); + std::vector ransacOutliers = keypoints.getRansacOutliers(); - for (std::vector::const_iterator it = - ransacInliers.begin(); - it != ransacInliers.end(); ++it) { + for (std::vector::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()); @@ -403,9 +380,8 @@ int main(int argc, const char **argv) vpDisplay::displayCircle(IMatching, imPt, 4, vpColor::green); } - for (std::vector::const_iterator it = - ransacOutliers.begin(); - it != ransacOutliers.end(); ++it) { + for (std::vector::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()); @@ -417,14 +393,12 @@ int main(int argc, const char **argv) // Display model in the correct sub-image in IMatching vpCameraParameters cam2; - cam2.initPersProjWithoutDistortion(cam.get_px(), cam.get_py(), - cam.get_u0() + I.getWidth(), + 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); + vpDisplay::displayFrame(IMatching, cMo, cam2, 0.025, vpColor::none, 3); } } diff --git a/modules/vision/test/key-point/testKeyPoint-3.cpp b/modules/vision/test/key-point/testKeyPoint-3.cpp index 1b26a58c0f..05401ff9b6 100644 --- a/modules/vision/test/key-point/testKeyPoint-3.cpp +++ b/modules/vision/test/key-point/testKeyPoint-3.cpp @@ -59,8 +59,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -105,8 +104,7 @@ OPTIONS: \n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -177,11 +175,9 @@ int main(int argc, const char **argv) std::string dirname = vpIoTools::createFilePath(env_ipath, "mbt/cube"); // Build the name of the image files - std::string filenameRef = - vpIoTools::createFilePath(dirname, "image0000.pgm"); + std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm"); vpImageIo::read(Iref, filenameRef); - std::string filenameCur = - vpIoTools::createFilePath(dirname, "image%04d.pgm"); + std::string filenameCur = vpIoTools::createFilePath(dirname, "image%04d.pgm"); // Init keypoints cv::Ptr detector; @@ -246,9 +242,8 @@ int main(int argc, const char **argv) std::vector > knn_matches; std::vector matches; matcher->knnMatch(queryDescriptors, trainDescriptors, knn_matches, 2); - for (std::vector >::const_iterator it = - knn_matches.begin(); - it != knn_matches.end(); ++it) { + for (std::vector >::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) { @@ -258,13 +253,10 @@ int main(int argc, const char **argv) } if (opt_display) { - for (std::vector::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); + for (std::vector::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()); + queryKeyPoints[(size_t)it->queryIdx].pt.x + Iref.getWidth()); vpDisplay::displayLine(Imatch, leftPt, rightPt, vpColor::green); } diff --git a/modules/vision/test/key-point/testKeyPoint-4.cpp b/modules/vision/test/key-point/testKeyPoint-4.cpp index 7b2aa4f4fa..ab9135e025 100644 --- a/modules/vision/test/key-point/testKeyPoint-4.cpp +++ b/modules/vision/test/key-point/testKeyPoint-4.cpp @@ -62,8 +62,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -109,8 +108,7 @@ OPTIONS: \n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -181,12 +179,10 @@ int main(int argc, const char **argv) std::string dirname = vpIoTools::createFilePath(env_ipath, "mbt/cube"); // Build the name of the image files - std::string filenameRef = - vpIoTools::createFilePath(dirname, "image0000.pgm"); + std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm"); vpImageIo::read(I, filenameRef); Iref = I; - std::string filenameCur = - vpIoTools::createFilePath(dirname, "image%04d.pgm"); + std::string filenameCur = vpIoTools::createFilePath(dirname, "image%04d.pgm"); #if defined VISP_HAVE_X11 vpDisplayX display, display2; @@ -204,17 +200,13 @@ int main(int argc, const char **argv) Imatch.resize(I.getHeight(), 2 * I.getWidth()); Imatch.insert(I, vpImagePoint(0, 0)); display2.setDownScalingFactor(vpDisplay::SCALE_AUTO); - display2.init(Imatch, 0, - (int)I.getHeight() / vpDisplay::getDownScalingFactor(I) + - 70, - "ORB keypoints matching"); + display2.init(Imatch, 0, (int)I.getHeight() / vpDisplay::getDownScalingFactor(I) + 70, "ORB keypoints matching"); } vpCameraParameters cam; vpMbEdgeTracker tracker; // Load config for tracker - std::string tracker_config_file = - vpIoTools::createFilePath(env_ipath, "mbt/cube.xml"); + std::string tracker_config_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml"); #ifdef VISP_HAVE_XML2 tracker.loadConfigFile(tracker_config_file); @@ -230,8 +222,7 @@ int main(int argc, const char **argv) me.setSampleStep(4); me.setNbTotalSample(250); tracker.setMovingEdge(me); - cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, - 234.5083345); + cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, 234.5083345); tracker.setCameraParameters(cam); tracker.setNearClippingDistance(0.01); tracker.setFarClippingDistance(100.0); @@ -242,18 +233,15 @@ int main(int argc, const char **argv) tracker.setAngleDisappear(vpMath::rad(89)); // Load CAO model - std::string cao_model_file = - vpIoTools::createFilePath(env_ipath, "mbt/cube.cao"); + std::string cao_model_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.cao"); tracker.loadModel(cao_model_file); // Initialize the pose - std::string init_file = - vpIoTools::createFilePath(env_ipath, "mbt/cube.init"); + std::string init_file = vpIoTools::createFilePath(env_ipath, "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); + vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); tracker.initFromPose(I, cMoi); } @@ -275,8 +263,7 @@ int main(int argc, const char **argv) #endif matcher = cv::DescriptorMatcher::create("BruteForce-Hamming"); -#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && \ - VISP_HAVE_OPENCV_VERSION < 0x030000) +#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000) detector->set("nLevels", 1); #endif @@ -289,22 +276,19 @@ int main(int argc, const char **argv) // Keep only keypoints on the cube std::vector polygons; std::vector > roisPt; - std::pair, std::vector > > - pair = tracker.getPolygonFaces(false); + std::pair, std::vector > > pair = tracker.getPolygonFaces(false); polygons = pair.first; roisPt = pair.second; // Compute the 3D coordinates std::vector points3f; - vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, - polygons, roisPt, 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()) { + if (trainKeyPoints.size() != (size_t)trainDescriptors.rows || trainKeyPoints.size() != points3f.size()) { std::cerr << "Problem with training data size !" << std::endl; return -1; } @@ -317,8 +301,7 @@ int main(int argc, const char **argv) bool opt_click = false; vpMouseButton::vpMouseButtonType button; - while ((opt_display && !g.end()) || - (!opt_display && g.getFrameIndex() < 30)) { + while ((opt_display && !g.end()) || (!opt_display && g.getFrameIndex() < 30)) { g.acquire(I); vpImageConvert::convert(I, matImg); @@ -331,9 +314,8 @@ int main(int argc, const char **argv) std::vector > knn_matches; std::vector matches; matcher->knnMatch(queryDescriptors, trainDescriptors, knn_matches, 2); - for (std::vector >::const_iterator it = - knn_matches.begin(); - it != knn_matches.end(); ++it) { + for (std::vector >::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) { @@ -343,16 +325,13 @@ int main(int argc, const char **argv) } vpPose estimated_pose; - for (std::vector::const_iterator it = matches.begin(); - it != matches.end(); ++it) { - vpPoint pt(points3f[(size_t)(it->trainIdx)].x, - points3f[(size_t)(it->trainIdx)].y, + for (std::vector::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); + 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); @@ -378,13 +357,10 @@ int main(int argc, const char **argv) Imatch.insert(I, vpImagePoint(0, Iref.getWidth())); vpDisplay::display(Imatch); - for (std::vector::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); + for (std::vector::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()); + queryKeyPoints[(size_t)it->queryIdx].pt.x + Iref.getWidth()); vpDisplay::displayLine(Imatch, leftPt, rightPt, vpColor::green); } diff --git a/modules/vision/test/key-point/testKeyPoint-5.cpp b/modules/vision/test/key-point/testKeyPoint-5.cpp index 49ca15b746..3c2b955d2a 100644 --- a/modules/vision/test/key-point/testKeyPoint-5.cpp +++ b/modules/vision/test/key-point/testKeyPoint-5.cpp @@ -57,8 +57,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -104,8 +103,7 @@ OPTIONS: \n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -196,12 +194,11 @@ int main(int argc, const char **argv) // Here, we want to test feature detection on a pyramid of images even for // features that are scale invariant to detect potential problem in ViSP. std::cout << "INFORMATION: " << std::endl; - std::cout - << "Here, we want to test feature detection on a pyramid of images " - "even for features " - "that are scale invariant to detect potential problem in ViSP." - << std::endl - << std::endl; + std::cout << "Here, we want to test feature detection on a pyramid of images " + "even for features " + "that are scale invariant to detect potential problem in ViSP." + << std::endl + << std::endl; vpKeyPoint keyPoints; // Will test the different types of keypoints detection to see if there is @@ -216,8 +213,7 @@ int main(int argc, const char **argv) detectorNames.push_back("PyramidSimpleBlob"); detectorNames.push_back("SimpleBlob"); // In contrib modules -#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || \ - defined(VISP_HAVE_OPENCV_XFEATURES2D) +#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || defined(VISP_HAVE_OPENCV_XFEATURES2D) detectorNames.push_back("PyramidSTAR"); detectorNames.push_back("STAR"); #endif @@ -245,26 +241,22 @@ int main(int argc, const char **argv) detectorNames.push_back("SURF"); #endif - for (std::vector::const_iterator itd = detectorNames.begin(); - itd != detectorNames.end(); ++itd) { + for (std::vector::const_iterator itd = detectorNames.begin(); itd != detectorNames.end(); ++itd) { keyPoints.setDetector(*itd); std::vector kpts; keyPoints.detect(I, kpts); - std::cout << "Nb keypoints detected: " << kpts.size() << " for " << *itd - << " method." << std::endl; + 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; + std::cerr << "No keypoints detected with " << *itd << " and image: " << filename << "." << std::endl; return EXIT_FAILURE; } if (opt_display) { vpDisplay::display(I); - for (std::vector::const_iterator it = kpts.begin(); - it != kpts.end(); ++it) { + for (std::vector::const_iterator it = kpts.begin(); it != kpts.end(); ++it) { vpImagePoint imPt; imPt.set_uv(it->pt.x, it->pt.y); @@ -281,8 +273,7 @@ int main(int argc, const char **argv) std::cout << "\n\n"; - std::map - mapOfDetectorNames = keyPoints.getDetectorNames(); + std::map mapOfDetectorNames = keyPoints.getDetectorNames(); for (int i = 0; i < vpKeyPoint::DETECTOR_TYPE_SIZE; i++) { keyPoints.setDetector((vpKeyPoint::vpFeatureDetectorType)i); @@ -290,11 +281,9 @@ int main(int argc, const char **argv) keyPoints.detect(I, kpts); std::cout << "Nb keypoints detected: " << kpts.size() << " for " - << mapOfDetectorNames[(vpKeyPoint::vpFeatureDetectorType)i] - << " method." << std::endl; + << mapOfDetectorNames[(vpKeyPoint::vpFeatureDetectorType)i] << " method." << std::endl; if (kpts.empty()) { - std::cerr << "No keypoints detected with " - << mapOfDetectorNames[(vpKeyPoint::vpFeatureDetectorType)i] + std::cerr << "No keypoints detected with " << mapOfDetectorNames[(vpKeyPoint::vpFeatureDetectorType)i] << " method and image: " << filename << "." << std::endl; return EXIT_FAILURE; } @@ -302,8 +291,7 @@ int main(int argc, const char **argv) if (opt_display) { vpDisplay::display(I); - for (std::vector::const_iterator it = kpts.begin(); - it != kpts.end(); ++it) { + for (std::vector::const_iterator it = kpts.begin(); it != kpts.end(); ++it) { vpImagePoint imPt; imPt.set_uv(it->pt.x, it->pt.y); diff --git a/modules/vision/test/key-point/testKeyPoint-6.cpp b/modules/vision/test/key-point/testKeyPoint-6.cpp index 501be4408d..fac8dfad94 100644 --- a/modules/vision/test/key-point/testKeyPoint-6.cpp +++ b/modules/vision/test/key-point/testKeyPoint-6.cpp @@ -56,8 +56,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! Print the program options. @@ -101,8 +100,7 @@ OPTIONS: \n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -249,8 +247,7 @@ int main(int argc, const char **argv) #if (VISP_HAVE_OPENCV_VERSION >= 0x020403) descriptorNames.push_back("BRISK"); #endif -#if defined(VISP_HAVE_OPENCV_XFEATURES2D) || \ - (VISP_HAVE_OPENCV_VERSION < 0x030000) +#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"); @@ -260,8 +257,7 @@ int main(int argc, const char **argv) descriptorNames.push_back("DAISY"); descriptorNames.push_back("LATCH"); #endif -#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && \ - defined(VISP_HAVE_OPENCV_XFEATURES2D) +#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && defined(VISP_HAVE_OPENCV_XFEATURES2D) descriptorNames.push_back("VGG"); descriptorNames.push_back("BoostDesc"); #endif @@ -275,49 +271,38 @@ int main(int argc, const char **argv) std::vector kpts; keyPoints.detect(I, kpts); - std::cout << "Nb keypoints detected: " << kpts.size() << " for " - << detectorName << " method." << std::endl; + 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; + std::cerr << "No keypoints detected with " << detectorName << " and image:" << filename << "." << std::endl; return EXIT_FAILURE; } - for (std::vector::const_iterator itd = - descriptorNames.begin(); - itd != descriptorNames.end(); ++itd) { + for (std::vector::const_iterator itd = descriptorNames.begin(); itd != descriptorNames.end(); ++itd) { keyPoints.setExtractor(*itd); if (*itd == "KAZE") { detectorName = "KAZE"; keyPoints.setDetector(detectorName); keyPoints.detect(I, kpts); - std::cout << "Nb keypoints detected: " << kpts.size() << " for " - << detectorName << " method." << std::endl; + 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; + std::cerr << "No keypoints detected with " << detectorName << " and image:" << filename << "." << std::endl; return EXIT_FAILURE; } } else if (*itd == "AKAZE") { detectorName = "AKAZE"; keyPoints.setDetector(detectorName); keyPoints.detect(I, kpts); - std::cout << "Nb keypoints detected: " << kpts.size() << " for " - << detectorName << " method." << std::endl; + 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; + std::cerr << "No keypoints detected with " << detectorName << " and image:" << filename << "." << std::endl; return EXIT_FAILURE; } } else if (*itd == "BoostDesc") { -#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && \ - defined(VISP_HAVE_OPENCV_XFEATURES2D) - cv::Ptr boostDesc = - keyPoints.getExtractor("BoostDesc"); +#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && defined(VISP_HAVE_OPENCV_XFEATURES2D) + cv::Ptr boostDesc = keyPoints.getExtractor("BoostDesc"); // Init BIN BOOST descriptor for FAST keypoints - boostDesc = cv::xfeatures2d::BoostDesc::create( - cv::xfeatures2d::BoostDesc::BINBOOST_256, true, 5.0f); + boostDesc = cv::xfeatures2d::BoostDesc::create(cv::xfeatures2d::BoostDesc::BINBOOST_256, true, 5.0f); #endif } @@ -327,20 +312,17 @@ int main(int argc, const char **argv) 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; + << " (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; + std::cerr << "No descriptor extracted with " << *itd << " and image:" << filename << "." << std::endl; return EXIT_FAILURE; } if (opt_display) { vpDisplay::display(I); - for (std::vector::const_iterator it = kpts.begin(); - it != kpts.end(); ++it) { + for (std::vector::const_iterator it = kpts.begin(); it != kpts.end(); ++it) { vpImagePoint imPt; imPt.set_uv(it->pt.x, it->pt.y); @@ -357,56 +339,43 @@ int main(int argc, const char **argv) std::cout << "\n\n"; - std::map - mapOfDescriptorNames = keyPoints.getExtractorNames(); + std::map mapOfDescriptorNames = keyPoints.getExtractorNames(); for (int i = 0; i < vpKeyPoint::DESCRIPTOR_TYPE_SIZE; i++) { keyPoints.setExtractor((vpKeyPoint::vpFeatureDescriptorType)i); - if (mapOfDescriptorNames[(vpKeyPoint::vpFeatureDescriptorType)i] == - "KAZE") { + if (mapOfDescriptorNames[(vpKeyPoint::vpFeatureDescriptorType)i] == "KAZE") { detectorName = "KAZE"; keyPoints.setDetector(detectorName); keyPoints.detect(I, kpts); - std::cout << "Nb keypoints detected: " << kpts.size() << " for " - << detectorName << " method." << std::endl; + 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; + std::cerr << "No keypoints detected with " << detectorName << " and image:" << filename << "." << std::endl; return EXIT_FAILURE; } - } else if (mapOfDescriptorNames[( - vpKeyPoint::vpFeatureDescriptorType)i] == "AKAZE") { + } else if (mapOfDescriptorNames[(vpKeyPoint::vpFeatureDescriptorType)i] == "AKAZE") { detectorName = "AKAZE"; keyPoints.setDetector(detectorName); keyPoints.detect(I, kpts); - std::cout << "Nb keypoints detected: " << kpts.size() << " for " - << detectorName << " method." << std::endl; + 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; + std::cerr << "No keypoints detected with " << detectorName << " and image:" << filename << "." << std::endl; return EXIT_FAILURE; } - } else if (mapOfDescriptorNames[( - vpKeyPoint::vpFeatureDescriptorType)i] == "BoostDesc") { -#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && \ - defined(VISP_HAVE_OPENCV_XFEATURES2D) + } else if (mapOfDescriptorNames[(vpKeyPoint::vpFeatureDescriptorType)i] == "BoostDesc") { +#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && defined(VISP_HAVE_OPENCV_XFEATURES2D) detectorName = "FAST"; keyPoints.setDetector(detectorName); keyPoints.detect(I, kpts); - std::cout << "Nb keypoints detected: " << kpts.size() << " for " - << detectorName << " method." << std::endl; + 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; + std::cerr << "No keypoints detected with " << detectorName << " and image:" << filename << "." << std::endl; return EXIT_FAILURE; } - cv::Ptr boostDesc = - keyPoints.getExtractor("BoostDesc"); + cv::Ptr boostDesc = keyPoints.getExtractor("BoostDesc"); // Init BIN BOOST descriptor for FAST keypoints - boostDesc = cv::xfeatures2d::BoostDesc::create( - cv::xfeatures2d::BoostDesc::BINBOOST_256, true, 5.0f); + boostDesc = cv::xfeatures2d::BoostDesc::create(cv::xfeatures2d::BoostDesc::BINBOOST_256, true, 5.0f); #endif } @@ -415,25 +384,20 @@ int main(int argc, const char **argv) keyPoints.extract(I, kpts, descriptor); t = vpTime::measureTimeMs() - t; - std::cout - << "Descriptor: " << descriptor.rows << "x" << descriptor.cols - << " (rows x cols) ; type=" << getOpenCVType(descriptor.type()) - << " for " - << mapOfDescriptorNames[(vpKeyPoint::vpFeatureDescriptorType)i] - << " method in " << t << " ms." << std::endl; + std::cout << "Descriptor: " << descriptor.rows << "x" << descriptor.cols + << " (rows x cols) ; type=" << getOpenCVType(descriptor.type()) << " for " + << mapOfDescriptorNames[(vpKeyPoint::vpFeatureDescriptorType)i] << " method in " << t << " ms." + << std::endl; if (descriptor.empty()) { - std::cerr - << "No descriptor extracted with " - << mapOfDescriptorNames[(vpKeyPoint::vpFeatureDescriptorType)i] - << " and image:" << filename << "." << std::endl; + std::cerr << "No descriptor extracted with " << mapOfDescriptorNames[(vpKeyPoint::vpFeatureDescriptorType)i] + << " and image:" << filename << "." << std::endl; return EXIT_FAILURE; } if (opt_display) { vpDisplay::display(I); - for (std::vector::const_iterator it = kpts.begin(); - it != kpts.end(); ++it) { + for (std::vector::const_iterator it = kpts.begin(); it != kpts.end(); ++it) { vpImagePoint imPt; imPt.set_uv(it->pt.x, it->pt.y); diff --git a/modules/vision/test/key-point/testKeyPoint-7.cpp b/modules/vision/test/key-point/testKeyPoint-7.cpp index f70f14aeba..ff01e01bb3 100644 --- a/modules/vision/test/key-point/testKeyPoint-7.cpp +++ b/modules/vision/test/key-point/testKeyPoint-7.cpp @@ -61,8 +61,7 @@ \param badparam : Bad parameter name. */ -void usage(const char *name, const char *badparam, const std::string &opath, - const std::string &user) +void usage(const char *name, const char *badparam, const std::string &opath, const std::string &user) { fprintf(stdout, "\n\ Test save / load learning files for vpKeyPoint class.\n\ @@ -97,8 +96,7 @@ OPTIONS: \n\ \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) +bool getOptions(int argc, const char **argv, std::string &opath, const std::string &user) { const char *optarg_; int c; @@ -145,70 +143,52 @@ bool getOptions(int argc, const char **argv, std::string &opath, \return True if the two vectors are identical, false otherwise. */ -bool compareKeyPoints(const std::vector &keypoints1, - const std::vector &keypoints2) +bool compareKeyPoints(const std::vector &keypoints1, const std::vector &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::epsilon())) { - std::cerr << std::fixed << std::setprecision(9) - << "keypoints1[cpt].angle=" << keypoints1[cpt].angle - << " ; keypoints2[cpt].angle=" << keypoints2[cpt].angle - << std::endl; + if (!vpMath::equal(keypoints1[cpt].angle, keypoints2[cpt].angle, std::numeric_limits::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; + << " ; 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; + << " ; keypoints2[cpt].octave=" << keypoints2[cpt].octave << std::endl; return false; } - if (!vpMath::equal(keypoints1[cpt].pt.x, keypoints2[cpt].pt.x, - std::numeric_limits::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; + if (!vpMath::equal(keypoints1[cpt].pt.x, keypoints2[cpt].pt.x, std::numeric_limits::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::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; + if (!vpMath::equal(keypoints1[cpt].pt.y, keypoints2[cpt].pt.y, std::numeric_limits::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::epsilon())) { - std::cerr << std::fixed << std::setprecision(9) - << "keypoints1[cpt].response=" << keypoints1[cpt].response - << " ; keypoints2[cpt].response=" << keypoints2[cpt].response - << std::endl; + if (!vpMath::equal(keypoints1[cpt].response, keypoints2[cpt].response, std::numeric_limits::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::epsilon())) { - std::cerr << std::fixed << std::setprecision(9) - << "keypoints1[cpt].size=" << keypoints1[cpt].size - << " ; keypoints2[cpt].size=" << keypoints2[cpt].size - << std::endl; + if (!vpMath::equal(keypoints1[cpt].size, keypoints2[cpt].size, std::numeric_limits::epsilon())) { + std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].size=" << keypoints1[cpt].size + << " ; keypoints2[cpt].size=" << keypoints2[cpt].size << std::endl; return false; } } @@ -224,11 +204,9 @@ bool compareKeyPoints(const std::vector &keypoints1, \return True if the two vectors are identical, false otherwise. */ -bool compareDescriptors(const cv::Mat &descriptors1, - const cv::Mat &descriptors2) +bool compareDescriptors(const cv::Mat &descriptors1, const cv::Mat &descriptors2) { - if (descriptors1.rows != descriptors2.rows || - descriptors1.cols != descriptors2.cols || + if (descriptors1.rows != descriptors2.rows || descriptors1.cols != descriptors2.cols || descriptors1.type() != descriptors2.type()) { return false; } @@ -237,79 +215,61 @@ bool compareDescriptors(const cv::Mat &descriptors1, for (int j = 0; j < descriptors1.cols; j++) { switch (descriptors1.type()) { case CV_8U: - if (descriptors1.at(i, j) != - descriptors2.at(i, j)) { - std::cerr << "descriptors1.at(i,j)=" - << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" - << descriptors2.at(i, j) << std::endl; + if (descriptors1.at(i, j) != descriptors2.at(i, j)) { + std::cerr << "descriptors1.at(i,j)=" << descriptors1.at(i, j) + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; case CV_8S: if (descriptors1.at(i, j) != descriptors2.at(i, j)) { - std::cerr << "descriptors1.at(i,j)=" - << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" - << descriptors2.at(i, j) << std::endl; + std::cerr << "descriptors1.at(i,j)=" << descriptors1.at(i, j) + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; case CV_16U: - if (descriptors1.at(i, j) != - descriptors2.at(i, j)) { - std::cerr << "descriptors1.at(i,j)=" - << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" - << descriptors2.at(i, j) << std::endl; + if (descriptors1.at(i, j) != descriptors2.at(i, j)) { + std::cerr << "descriptors1.at(i,j)=" << descriptors1.at(i, j) + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; case CV_16S: if (descriptors1.at(i, j) != descriptors2.at(i, j)) { - std::cerr << "descriptors1.at(i,j)=" - << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" - << descriptors2.at(i, j) << std::endl; + std::cerr << "descriptors1.at(i,j)=" << descriptors1.at(i, j) + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; case CV_32S: if (descriptors1.at(i, j) != descriptors2.at(i, j)) { - std::cerr << "descriptors1.at(i,j)=" - << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" - << descriptors2.at(i, j) << std::endl; + std::cerr << "descriptors1.at(i,j)=" << descriptors1.at(i, j) + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; case CV_32F: - if (!vpMath::equal(descriptors1.at(i, j), - descriptors2.at(i, j), + if (!vpMath::equal(descriptors1.at(i, j), descriptors2.at(i, j), std::numeric_limits::epsilon())) { std::cerr << std::fixed << std::setprecision(9) - << "descriptors1.at(i,j)=" - << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" - << descriptors2.at(i, j) << std::endl; + << "descriptors1.at(i,j)=" << descriptors1.at(i, j) + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; case CV_64F: - if (!vpMath::equal(descriptors1.at(i, j), - descriptors2.at(i, j), + if (!vpMath::equal(descriptors1.at(i, j), descriptors2.at(i, j), std::numeric_limits::epsilon())) { std::cerr << std::fixed << std::setprecision(17) - << "descriptors1.at(i,j)=" - << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" - << descriptors2.at(i, j) << std::endl; + << "descriptors1.at(i,j)=" << descriptors1.at(i, j) + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; @@ -343,9 +303,7 @@ int main(int argc, const char **argv) env_ipath = vpIoTools::getViSPImagesDataPath(); if (env_ipath.empty()) { - throw vpException( - vpException::ioError, - "Please set the VISP_INPUT_IMAGE_PATH environment variable value."); + throw vpException(vpException::ioError, "Please set the VISP_INPUT_IMAGE_PATH environment variable value."); } // Set the default output path @@ -360,9 +318,7 @@ int main(int argc, const char **argv) // Read the command line options if (getOptions(argc, argv, opt_opath, username) == false) { - throw vpException( - vpException::fatalError, - "getOptions(argc, argv, opt_opath, username) == false"); + throw vpException(vpException::fatalError, "getOptions(argc, argv, opt_opath, username) == false"); } // Get the option values @@ -379,8 +335,7 @@ int main(int argc, const char **argv) std::string dirname = vpIoTools::createFilePath(env_ipath, "Klimt"); // Build the name of the image files - std::string img_filename = - vpIoTools::createFilePath(dirname, "/Klimt.ppm"); + std::string img_filename = vpIoTools::createFilePath(dirname, "/Klimt.ppm"); vpImageIo::read(I, img_filename); vpKeyPoint keyPoints; @@ -396,8 +351,7 @@ int main(int argc, const char **argv) std::vector trainKeyPoints; keyPoints.getTrainKeyPoints(trainKeyPoints); cv::Mat trainDescriptors = keyPoints.getTrainDescriptors(); - if (trainKeyPoints.empty() || trainDescriptors.empty() || - (int)trainKeyPoints.size() != trainDescriptors.rows) { + if (trainKeyPoints.empty() || trainDescriptors.empty() || (int)trainKeyPoints.size() != trainDescriptors.rows) { throw vpException(vpException::fatalError, "Problem when detecting " "keypoints or when " "computing descriptors !"); @@ -406,8 +360,7 @@ int main(int argc, const char **argv) // 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"); + filename = vpIoTools::createFilePath(filename, "test_save_in_bin_with_img.bin"); keyPoints.saveLearningData(filename, true, true); // Test if save is ok @@ -425,24 +378,20 @@ int main(int argc, const char **argv) 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 !"); + 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 !"); + 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"); + filename = vpIoTools::createFilePath(filename, "test_save_in_bin_without_img.bin"); keyPoints.saveLearningData(filename, true, false); // Test if save is ok @@ -460,25 +409,21 @@ int main(int argc, const char **argv) 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 !"); + 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 !"); + 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"); + filename = vpIoTools::createFilePath(filename, "test_save_in_xml_with_img.xml"); keyPoints.saveLearningData(filename, false, true); // Test if save is ok @@ -496,24 +441,20 @@ int main(int argc, const char **argv) 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 !"); + 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 !"); + 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"); + filename = vpIoTools::createFilePath(filename, "test_save_in_xml_without_img.xml"); keyPoints.saveLearningData(filename, false, false); // Test if save is ok @@ -531,29 +472,23 @@ int main(int argc, const char **argv) 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 !"); + 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 !"); + 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; + 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)) +#if defined(VISP_HAVE_OPENCV_NONFREE) || \ + ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(VISP_HAVE_OPENCV_XFEATURES2D)) { std::string keypointName = "SIFT"; keyPoints.setDetector(keypointName); @@ -564,18 +499,15 @@ int main(int argc, const char **argv) std::vector 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) !"); + 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"); + filename = vpIoTools::createFilePath(filename, "test_save_in_bin_with_img.bin"); keyPoints.saveLearningData(filename, true, true); // Test if save is ok @@ -593,24 +525,20 @@ int main(int argc, const char **argv) 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 !"); + 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 !"); + 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"); + filename = vpIoTools::createFilePath(filename, "test_save_in_bin_without_img.bin"); keyPoints.saveLearningData(filename, true, false); // Test if save is ok @@ -628,25 +556,21 @@ int main(int argc, const char **argv) 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 !"); + 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 !"); + 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"); + filename = vpIoTools::createFilePath(filename, "test_save_in_xml_with_img.xml"); keyPoints.saveLearningData(filename, false, true); // Test if save is ok @@ -664,24 +588,20 @@ int main(int argc, const char **argv) 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 !"); + 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 !"); + 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"); + filename = vpIoTools::createFilePath(filename, "test_save_in_xml_without_img.xml"); keyPoints.saveLearningData(filename, false, false); // Test if save is ok @@ -699,17 +619,14 @@ int main(int argc, const char **argv) 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 !"); + 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 !"); + throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading " + "learning file saved in " + "xml without train images saved !"); } #endif @@ -742,15 +659,11 @@ int main(int argc, const char **argv) // 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 !"); + 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 !"); + throw vpException(vpException::fatalError, "Problem with vpKeyPoint::reset() and trainDescriptors !"); } std::cout << "vpKeyPoint::reset() is ok with trainKeyPoints and " diff --git a/modules/vision/test/key-point/testKeyPoint.cpp b/modules/vision/test/key-point/testKeyPoint.cpp index a5a8a96a5d..1d277450ee 100644 --- a/modules/vision/test/key-point/testKeyPoint.cpp +++ b/modules/vision/test/key-point/testKeyPoint.cpp @@ -57,8 +57,7 @@ #define GETOPTARGS "cdh" void usage(const char *name, const char *badparam); -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display); +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display); /*! @@ -104,8 +103,7 @@ OPTIONS: \n\ \return false if the program has to be stopped, true otherwise. */ -bool getOptions(int argc, const char **argv, bool &click_allowed, - bool &display) +bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -175,16 +173,13 @@ int main(int argc, const char **argv) std::string dirname = vpIoTools::createFilePath(env_ipath, "mbt/cube"); // Build the name of the image files - std::string filenameRef = - vpIoTools::createFilePath(dirname, "image0000.pgm"); + std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm"); vpImageIo::read(Iref, filenameRef); - std::string filenameCur = - vpIoTools::createFilePath(dirname, "image%04d.pgm"); + 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; + std::cout << "Build " << keypoints.buildReference(Iref) << " reference points." << std::endl; vpVideoReader g; g.setFileName(filenameCur); diff --git a/modules/vision/test/key-point/testSurfKeyPoint.cpp b/modules/vision/test/key-point/testSurfKeyPoint.cpp index 8962820edd..10bbb6de96 100644 --- a/modules/vision/test/key-point/testSurfKeyPoint.cpp +++ b/modules/vision/test/key-point/testSurfKeyPoint.cpp @@ -44,9 +44,7 @@ #include -#if (defined(VISP_HAVE_OPENCV_NONFREE) && \ - (VISP_HAVE_OPENCV_VERSION < \ - 0x030000)) // Require opencv >= 1.1.0 < 3.0.0 +#if (defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)) // Require opencv >= 1.1.0 < 3.0.0 #include #include @@ -69,8 +67,7 @@ #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); +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display); /*! @@ -124,8 +121,7 @@ OPTIONS: Default\n\ \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) +bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display) { const char *optarg_; int c; @@ -185,8 +181,7 @@ int main(int argc, const char **argv) ipath = env_ipath; // Read the command line options - if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == - false) { + if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) { exit(-1); } @@ -200,8 +195,7 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath - << std::endl + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl << " we skip the environment variable." << std::endl; } } @@ -210,13 +204,10 @@ int main(int argc, const char **argv) if (opt_ipath.empty() && env_ipath.empty()) { usage(argv[0], NULL, ipath); std::cerr << std::endl << "ERROR:" << std::endl; - std::cerr - << " Use -i 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; + std::cerr << " Use -i 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); } @@ -253,11 +244,9 @@ int main(int argc, const char **argv) // 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 << " 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; + << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl; exit(-1); } @@ -321,8 +310,7 @@ int main(int argc, const char **argv) unsigned int nbrPair; if (opt_display) { - display[1].init(Icur, (int)(100 + Iref.getWidth()), 100, - "Current image"); + display[1].init(Icur, (int)(100 + Iref.getWidth()), 100, "Current image"); // display variable. vpDisplay::display(Icur); // Flush the display @@ -340,8 +328,7 @@ int main(int argc, const char **argv) for (int i = 0; i < 2; i++) { vpDisplay::getClick(Icur, corners[i]); } - vpDisplay::displayRectangle(Icur, corners[0], corners[1], - vpColor::green); + 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()); diff --git a/modules/vision/test/pose/testFindMatch.cpp b/modules/vision/test/pose/testFindMatch.cpp index da1e883915..9310002208 100644 --- a/modules/vision/test/pose/testFindMatch.cpp +++ b/modules/vision/test/pose/testFindMatch.cpp @@ -67,8 +67,7 @@ int main() 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)); + vpHomogeneousMatrix cMo_ref(0, 0.2, 1, vpMath::rad(3), vpMath::rad(-2), vpMath::rad(10)); std::vector p(P.size()); for (unsigned int i = 0; i < P.size(); i++) { @@ -85,8 +84,7 @@ int main() vpHomogeneousMatrix cMo; - vpPose::findMatch(p, P, nbInlierToReachConsensus, threshold, ninliers, - inliers, cMo); + vpPose::findMatch(p, P, nbInlierToReachConsensus, threshold, ninliers, inliers, cMo); std::cout << "Inliers: " << std::endl; for (unsigned int i = 0; i < inliers.size(); i++) { @@ -100,12 +98,8 @@ int main() 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; + 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++) { @@ -113,8 +107,7 @@ int main() test_fail = 1; } - std::cout << "Matching is " << (test_fail ? "badly" : "well") - << " performed" << std::endl; + std::cout << "Matching is " << (test_fail ? "badly" : "well") << " performed" << std::endl; return test_fail; } catch (vpException &e) { diff --git a/modules/vision/test/pose/testPose.cpp b/modules/vision/test/pose/testPose.cpp index a707b98ccf..b63520fbe1 100644 --- a/modules/vision/test/pose/testPose.cpp +++ b/modules/vision/test/pose/testPose.cpp @@ -61,8 +61,7 @@ */ void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend); -int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, - const vpHomogeneousMatrix &cMo_est, +int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est, const std::string &legend); // print the resulting estimated pose @@ -82,8 +81,7 @@ void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend) } // test if pose is well estimated -int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, - const vpHomogeneousMatrix &cMo_est, +int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est, const std::string &legend) { vpPoseVector pose_ref = vpPoseVector(cMo_ref); @@ -97,8 +95,7 @@ int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, fail = 1; } - std::cout << "Based on 3D parameters " << legend << " is " - << (fail ? "badly" : "well") << " estimated" << std::endl; + 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); @@ -110,8 +107,8 @@ int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, 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; + std::cout << "Based on 2D residual (" << r << ") " << legend << " is " << (fail ? "badly" : "well") << " estimated" + << std::endl; return fail; } @@ -129,8 +126,7 @@ int main() 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)); + 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 @@ -144,24 +140,21 @@ int main() print_pose(cMo_ref, std::string("Reference pose")); // print the reference pose - std::cout << "-------------------------------------------------" - << std::endl; + 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; + 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; + std::cout << "--------------------------------------------------" << std::endl; pose.setRansacNbInliersToReachConsensus(4); pose.setRansacThreshold(0.01); pose.computePose(vpPose::RANSAC, cMo); @@ -170,16 +163,14 @@ int main() fail = compare_pose(pose, cMo_ref, cMo, "pose by Ransac"); test_fail |= fail; - std::cout << "--------------------------------------------------" - << std::endl; + 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; + std::cout << "--------------------------------------------------" << std::endl; pose.computePose(vpPose::DEMENTHON_LOWE, cMo); print_pose(cMo, std::string("Pose estimated by Dementhon than Lowe")); @@ -188,32 +179,28 @@ int main() // Now Virtual Visual servoing - std::cout << "--------------------------------------------------" - << std::endl; + 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; + 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; + 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; + std::cout << "\nGlobal pose estimation test " << (test_fail ? "fail" : "is ok") << std::endl; return test_fail; } catch (vpException &e) { diff --git a/modules/vision/test/pose/testPoseFeatures.cpp b/modules/vision/test/pose/testPoseFeatures.cpp index f82e236013..64174100a9 100644 --- a/modules/vision/test/pose/testPoseFeatures.cpp +++ b/modules/vision/test/pose/testPoseFeatures.cpp @@ -73,34 +73,25 @@ class vp_createPointClass } }; -void vp_createPoint(vpFeaturePoint &fp, const vpPoint &v) -{ - vpFeatureBuilder::create(fp, v); -} +void vp_createPoint(vpFeaturePoint &fp, const vpPoint &v) { vpFeatureBuilder::create(fp, v); } -void vp_createLine(vpFeatureLine &fp, const vpLine &v) -{ - vpFeatureBuilder::create(fp, v); -} +void vp_createLine(vpFeatureLine &fp, const vpLine &v) { vpFeatureBuilder::create(fp, v); } #endif #endif int test_pose(bool use_robust) { if (use_robust) - std::cout << "** Test robust pose estimation from features\n" - << std::endl; + std::cout << "** Test robust pose estimation from features\n" << std::endl; else std::cout << "** Test pose estimation from features\n" << std::endl; vpImage I(600, 600); - vpHomogeneousMatrix cMo_ref(0., 0., 1., vpMath::rad(0), vpMath::rad(0), - vpMath::rad(60)); + 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 << "Reference pose used to create the visual features : " << std::endl; std::cout << pose_ref.t() << std::endl; vpPoseFeatures pose; @@ -171,11 +162,9 @@ int test_pose(bool use_robust) vpFeaturePoint fp; vpFeatureLine fl; vpFeatureSegment fs; - void (*ptr)(vpFeatureSegment &, vpPoint &, vpPoint &) = - &vpFeatureBuilder::create; + void (*ptr)(vpFeatureSegment &, vpPoint &, vpPoint &) = &vpFeatureBuilder::create; vp_createPointClass cpClass; - int (vp_createPointClass::*ptrClass)(vpFeaturePoint &, const vpPoint &) = - &vp_createPointClass::vp_createPoint; + 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]); @@ -186,11 +175,9 @@ int test_pose(bool use_robust) pose.setVVSIterMax(200); pose.setCovarianceComputation(true); - vpHomogeneousMatrix cMo_est(0.4, 0.3, 1.5, vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + 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 << "\nPose used as initialisation of the pose computation : " << std::endl; std::cout << pose_est.t() << std::endl; if (!use_robust) @@ -201,18 +188,15 @@ int test_pose(bool use_robust) if (!use_robust) std::cout << "\nEstimated pose from visual features : " << std::endl; else - std::cout << "\nRobust estimated pose from visual features : " - << std::endl; + std::cout << "\nRobust estimated 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; + 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++) { @@ -220,9 +204,7 @@ int test_pose(bool use_robust) test_fail = 1; } - std::cout << "\nPose is " << (test_fail ? "badly" : "well") - << " estimated\n" - << std::endl; + std::cout << "\nPose is " << (test_fail ? "badly" : "well") << " estimated\n" << std::endl; return test_fail; } diff --git a/modules/vision/test/pose/testPoseRansac.cpp b/modules/vision/test/pose/testPoseRansac.cpp index 3c661e72f6..7d9e542bf5 100644 --- a/modules/vision/test/pose/testPoseRansac.cpp +++ b/modules/vision/test/pose/testPoseRansac.cpp @@ -88,8 +88,7 @@ int main() 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); + unsigned int nbInlierToReachConsensus = (unsigned int)(75.0 * (double)(P.size()) / 100.0); double threshold = 0.001; pose.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus); @@ -111,12 +110,8 @@ int main() 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; + 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++) { @@ -124,8 +119,7 @@ int main() test_fail = 1; } - std::cout << "Pose is " << (test_fail ? "badly" : "well") << " estimated" - << std::endl; + 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; diff --git a/modules/vision/test/pose/testPoseRansac2.cpp b/modules/vision/test/pose/testPoseRansac2.cpp index 875ea16b58..2aca266dc8 100644 --- a/modules/vision/test/pose/testPoseRansac2.cpp +++ b/modules/vision/test/pose/testPoseRansac2.cpp @@ -1674,26 +1674,19 @@ const std::string file_content = "0.038043 0.109755 0.016169 " bool samePoints(const vpPoint &pt1, const vpPoint &pt2) { - return vpMath::equal(pt1.get_oX(), pt2.get_oX(), - std::numeric_limits::epsilon()) && - vpMath::equal(pt1.get_oY(), pt2.get_oY(), - std::numeric_limits::epsilon()) && - vpMath::equal(pt1.get_oZ(), pt2.get_oZ(), - std::numeric_limits::epsilon()) && - vpMath::equal(pt1.get_x(), pt2.get_x(), - std::numeric_limits::epsilon()) && - vpMath::equal(pt1.get_y(), pt2.get_y(), - std::numeric_limits::epsilon()); + return vpMath::equal(pt1.get_oX(), pt2.get_oX(), std::numeric_limits::epsilon()) && + vpMath::equal(pt1.get_oY(), pt2.get_oY(), std::numeric_limits::epsilon()) && + vpMath::equal(pt1.get_oZ(), pt2.get_oZ(), std::numeric_limits::epsilon()) && + vpMath::equal(pt1.get_x(), pt2.get_x(), std::numeric_limits::epsilon()) && + vpMath::equal(pt1.get_y(), pt2.get_y(), std::numeric_limits::epsilon()); } -int checkInlierIndex( - const std::vector &vectorOfFoundInlierIndex, - const std::vector &vectorOfOutlierFlags) +int checkInlierIndex(const std::vector &vectorOfFoundInlierIndex, + const std::vector &vectorOfOutlierFlags) { int nbInlierIndexOk = 0; - for (std::vector::const_iterator it = - vectorOfFoundInlierIndex.begin(); + for (std::vector::const_iterator it = vectorOfFoundInlierIndex.begin(); it != vectorOfFoundInlierIndex.end(); ++it) { if (!vectorOfOutlierFlags[*it]) { nbInlierIndexOk++; @@ -1703,14 +1696,12 @@ int checkInlierIndex( return nbInlierIndexOk; } -bool checkInlierPoints( - const std::vector &vectorOfFoundInlierPoints, - const std::vector &vectorOfFoundInlierIndex, - const std::vector &bunnyModelPoints_noisy) +bool checkInlierPoints(const std::vector &vectorOfFoundInlierPoints, + const std::vector &vectorOfFoundInlierIndex, + const std::vector &bunnyModelPoints_noisy) { for (size_t i = 0; i < vectorOfFoundInlierPoints.size(); i++) { - if (!samePoints(vectorOfFoundInlierPoints[i], - bunnyModelPoints_noisy[vectorOfFoundInlierIndex[i]])) { + if (!samePoints(vectorOfFoundInlierPoints[i], bunnyModelPoints_noisy[vectorOfFoundInlierIndex[i]])) { std::cerr << "Problem with the inlier index and the corresponding " "inlier point !" << std::endl; @@ -1721,8 +1712,7 @@ bool checkInlierPoints( return true; } -void readBunnyModelPoints(std::vector &bunnyModelPoints, - std::vector &bunnyModelPoints_noisy) +void readBunnyModelPoints(std::vector &bunnyModelPoints, std::vector &bunnyModelPoints_noisy) { // Read the string std::istringstream iss_file_content(file_content); @@ -1731,8 +1721,7 @@ void readBunnyModelPoints(std::vector &bunnyModelPoints, vpTranslationVector translation(-0.14568, 0.154567, 1.4462); // vpRzyxVector zyxVector(vpMath::rad(10.0f), vpMath::rad(-15.0f), // vpMath::rad(38.0f)); - vpRzyxVector zyxVector(vpMath::rad(12.4146f), vpMath::rad(-75.5478f), - vpMath::rad(138.5607f)); + vpRzyxVector zyxVector(vpMath::rad(12.4146f), vpMath::rad(-75.5478f), vpMath::rad(138.5607f)); vpThetaUVector thetau(zyxVector); vpHomogeneousMatrix cMo_groundTruth(translation, thetau); @@ -1753,21 +1742,16 @@ void readBunnyModelPoints(std::vector &bunnyModelPoints, } // Print the number of model points - std::cout << "The raw model contains " << bunnyModelPoints.size() - << " points." << std::endl; - std::cout << "cMo_groundTruth=\n" - << cMo_groundTruth << std::endl - << std::endl; + std::cout << "The raw model contains " << bunnyModelPoints.size() << " points." << std::endl; + std::cout << "cMo_groundTruth=\n" << cMo_groundTruth << std::endl << std::endl; } bool testRansac(const std::vector &bunnyModelPoints_original, - const std::vector &bunnyModelPoints_noisy_original, - const size_t nb_model_points, const bool test_duplicate, - const bool test_degenerate) + const std::vector &bunnyModelPoints_noisy_original, const size_t nb_model_points, + const bool test_duplicate, const bool test_degenerate) { std::vector bunnyModelPoints = bunnyModelPoints_original; - std::vector bunnyModelPoints_noisy = - bunnyModelPoints_noisy_original; + std::vector bunnyModelPoints_noisy = bunnyModelPoints_noisy_original; // Resize if (nb_model_points > 0) { bunnyModelPoints.resize(nb_model_points); @@ -1793,22 +1777,17 @@ bool testRansac(const std::vector &bunnyModelPoints_original, real_pose.computePose(vpPose::VIRTUAL_VS, cMo_estimated); double r_vvs = ground_truth_pose.computeResidual(cMo_estimated); - std::cout - << "\ncMo estimated using VVS on data with small gaussian noise:\n" - << cMo_estimated << std::endl; + std::cout << "\ncMo estimated using VVS on data with small gaussian noise:\n" << cMo_estimated << std::endl; std::cout << "Corresponding residual: " << r_vvs << std::endl; size_t nbOutliers = (size_t)(0.35 * bunnyModelPoints_noisy.size()); vpGaussRand noise(0.01, 0.008 /*, time(NULL)*/); // Vector that indicates if the point is an outlier or not - std::vector vectorOfOutlierFlags(bunnyModelPoints_noisy.size(), - false); + std::vector vectorOfOutlierFlags(bunnyModelPoints_noisy.size(), false); // Generate outliers points for (size_t i = 0; i < nbOutliers; i++) { - bunnyModelPoints_noisy[i].set_x(bunnyModelPoints_noisy[i].get_x() + - noise()); - bunnyModelPoints_noisy[i].set_y(bunnyModelPoints_noisy[i].get_y() + - noise()); + bunnyModelPoints_noisy[i].set_x(bunnyModelPoints_noisy[i].get_x() + noise()); + bunnyModelPoints_noisy[i].set_y(bunnyModelPoints_noisy[i].get_y() + noise()); vectorOfOutlierFlags[i] = true; } @@ -1853,8 +1832,7 @@ bool testRansac(const std::vector &bunnyModelPoints_original, listOfDegeneratePoints.push_back(degeneratePoint); } - for (std::vector::const_iterator it_degenerate = - listOfDegeneratePoints.begin(); + for (std::vector::const_iterator it_degenerate = listOfDegeneratePoints.begin(); it_degenerate != listOfDegeneratePoints.end(); ++it_degenerate) { bunnyModelPoints_noisy.push_back(*it_degenerate); vectorOfOutlierFlags.push_back(true); @@ -1872,8 +1850,7 @@ bool testRansac(const std::vector &bunnyModelPoints_original, bunnyModelPoints_noisy.clear(); std::vector vectorOfOutlierFlags_tmp = vectorOfOutlierFlags; vectorOfOutlierFlags.clear(); - for (std::vector::const_iterator it = vectorOfIndex.begin(); - it != vectorOfIndex.end(); ++it) { + for (std::vector::const_iterator it = vectorOfIndex.begin(); it != vectorOfIndex.end(); ++it) { bunnyModelPoints_noisy.push_back(bunnyModelPoints_noisy_tmp[*it]); vectorOfOutlierFlags.push_back(vectorOfOutlierFlags_tmp[*it]); } @@ -1882,8 +1859,7 @@ bool testRansac(const std::vector &bunnyModelPoints_original, vpPose pose; vpPose pose_ransac, pose_ransac2; -#if defined(VISP_HAVE_PTHREAD) || \ - (defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_OPENMP) +#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_OPENMP) #define TEST_PARALLEL_RANSAC #endif @@ -1899,10 +1875,8 @@ bool testRansac(const std::vector &bunnyModelPoints_original, pose_ransac_parallel.setUseParallelRansac(true); pose_ransac_parallel2.setUseParallelRansac(true); - pose_ransac_parallel.setRansacFilterFlags( - vpPose::PREFILTER_DUPLICATE_POINTS + vpPose::CHECK_DEGENERATE_POINTS); - pose_ransac_parallel2.setRansacFilterFlags( - vpPose::PREFILTER_DUPLICATE_POINTS + vpPose::CHECK_DEGENERATE_POINTS); + pose_ransac_parallel.setRansacFilterFlags(vpPose::PREFILTER_DUPLICATE_POINTS + vpPose::CHECK_DEGENERATE_POINTS); + pose_ransac_parallel2.setRansacFilterFlags(vpPose::PREFILTER_DUPLICATE_POINTS + vpPose::CHECK_DEGENERATE_POINTS); #if !defined(VISP_HAVE_OPENMP) pose_ransac_parallel.setNbParallelRansacThreads(2); @@ -1910,13 +1884,10 @@ bool testRansac(const std::vector &bunnyModelPoints_original, #endif } #endif - pose_ransac.setRansacFilterFlags(vpPose::PREFILTER_DUPLICATE_POINTS + - vpPose::CHECK_DEGENERATE_POINTS); - pose_ransac2.setRansacFilterFlags(vpPose::PREFILTER_DUPLICATE_POINTS + - vpPose::CHECK_DEGENERATE_POINTS); - for (std::vector::const_iterator it = - bunnyModelPoints_noisy.begin(); - it != bunnyModelPoints_noisy.end(); ++it) { + pose_ransac.setRansacFilterFlags(vpPose::PREFILTER_DUPLICATE_POINTS + vpPose::CHECK_DEGENERATE_POINTS); + pose_ransac2.setRansacFilterFlags(vpPose::PREFILTER_DUPLICATE_POINTS + vpPose::CHECK_DEGENERATE_POINTS); + for (std::vector::const_iterator it = bunnyModelPoints_noisy.begin(); it != bunnyModelPoints_noisy.end(); + ++it) { pose.addPoint(*it); } // Test addPoints @@ -1930,12 +1901,11 @@ bool testRansac(const std::vector &bunnyModelPoints_original, #endif // Print the number of points in the final data vector - std::cout << "\nNumber of model points in the noisy data vector: " - << bunnyModelPoints_noisy.size() << " points." << std::endl + std::cout << "\nNumber of model points in the noisy data vector: " << bunnyModelPoints_noisy.size() << " points." + << std::endl << std::endl; - unsigned int nbInlierToReachConsensus = - (unsigned int)(60.0 * (double)(bunnyModelPoints_noisy.size()) / 100.0); + unsigned int nbInlierToReachConsensus = (unsigned int)(60.0 * (double)(bunnyModelPoints_noisy.size()) / 100.0); double threshold = 0.001; // RANSAC with 1000 iterations @@ -1944,16 +1914,13 @@ bool testRansac(const std::vector &bunnyModelPoints_original, pose_ransac.setRansacMaxTrials(1000); #ifdef TEST_PARALLEL_RANSAC if (appveyor_threading == "true") { - pose_ransac_parallel.setRansacNbInliersToReachConsensus( - nbInlierToReachConsensus); + pose_ransac_parallel.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus); pose_ransac_parallel.setRansacThreshold(threshold); pose_ransac_parallel.setRansacMaxTrials(1000); - pose_ransac_parallel2.setRansacNbInliersToReachConsensus( - nbInlierToReachConsensus); + pose_ransac_parallel2.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus); pose_ransac_parallel2.setRansacThreshold(threshold); - pose_ransac_parallel2.setRansacMaxTrials( - vpPose::computeRansacIterations(0.99, 0.4, 4, -1)); + pose_ransac_parallel2.setRansacMaxTrials(vpPose::computeRansacIterations(0.99, 0.4, 4, -1)); } #endif @@ -1962,39 +1929,30 @@ bool testRansac(const std::vector &bunnyModelPoints_original, pose_ransac2.setRansacThreshold(threshold); int ransac_iterations = vpPose::computeRansacIterations(0.99, 0.4, 4, -1); pose_ransac2.setRansacMaxTrials(ransac_iterations); - std::cout - << "Number of RANSAC iterations to ensure p=0.99 and epsilon=0.4: " - << ransac_iterations << std::endl; + std::cout << "Number of RANSAC iterations to ensure p=0.99 and epsilon=0.4: " << ransac_iterations << std::endl; vpHomogeneousMatrix cMo_estimated_RANSAC; double t_RANSAC = vpTime::measureTimeMs(); pose_ransac.computePose(vpPose::RANSAC, cMo_estimated_RANSAC); t_RANSAC = vpTime::measureTimeMs() - t_RANSAC; - std::cout - << "\ncMo estimated with RANSAC (1000 iterations) on noisy data:\n" - << cMo_estimated_RANSAC << std::endl; + std::cout << "\ncMo estimated with RANSAC (1000 iterations) on noisy data:\n" << cMo_estimated_RANSAC << std::endl; std::cout << "Computation time: " << t_RANSAC << " ms" << std::endl; - double r_RANSAC_estimated = - ground_truth_pose.computeResidual(cMo_estimated_RANSAC); - std::cout << "Corresponding residual (1000 iterations): " - << r_RANSAC_estimated << std::endl; + double r_RANSAC_estimated = ground_truth_pose.computeResidual(cMo_estimated_RANSAC); + std::cout << "Corresponding residual (1000 iterations): " << r_RANSAC_estimated << std::endl; vpHomogeneousMatrix cMo_estimated_RANSAC_2; t_RANSAC = vpTime::measureTimeMs(); pose_ransac2.computePose(vpPose::RANSAC, cMo_estimated_RANSAC_2); t_RANSAC = vpTime::measureTimeMs() - t_RANSAC; - std::cout << "\ncMo estimated with RANSAC (" << ransac_iterations - << " iterations) on noisy data:\n" + std::cout << "\ncMo estimated with RANSAC (" << ransac_iterations << " iterations) on noisy data:\n" << cMo_estimated_RANSAC_2 << std::endl; std::cout << "Computation time: " << t_RANSAC << " ms" << std::endl; - double r_RANSAC_estimated_2 = - ground_truth_pose.computeResidual(cMo_estimated_RANSAC_2); - std::cout << "Corresponding residual (" << ransac_iterations - << " iterations): " << r_RANSAC_estimated_2 << std::endl; + double r_RANSAC_estimated_2 = ground_truth_pose.computeResidual(cMo_estimated_RANSAC_2); + std::cout << "Corresponding residual (" << ransac_iterations << " iterations): " << r_RANSAC_estimated_2 << std::endl; pose.computePose(vpPose::DEMENTHON, cMo_dementhon); pose.computePose(vpPose::LAGRANGE, cMo_lagrange); @@ -2008,8 +1966,7 @@ bool testRansac(const std::vector &bunnyModelPoints_original, } pose.computePose(vpPose::VIRTUAL_VS, cMo_estimated); - std::cout << "\ncMo estimated with only VVS on noisy data:\n" - << cMo_estimated << std::endl; + std::cout << "\ncMo estimated with only VVS on noisy data:\n" << cMo_estimated << std::endl; double r_estimated = ground_truth_pose.computeResidual(cMo_estimated); std::cout << "Corresponding residual: " << r_estimated << std::endl; @@ -2019,56 +1976,42 @@ bool testRansac(const std::vector &bunnyModelPoints_original, if (appveyor_threading == "true") { vpHomogeneousMatrix cMo_estimated_RANSAC_parallel; double t_RANSAC_parallel = vpTime::measureTimeMs(); - pose_ransac_parallel.computePose(vpPose::RANSAC, - cMo_estimated_RANSAC_parallel); + pose_ransac_parallel.computePose(vpPose::RANSAC, cMo_estimated_RANSAC_parallel); t_RANSAC_parallel = vpTime::measureTimeMs() - t_RANSAC_parallel; std::cout << "\ncMo estimated with parallel RANSAC (1000 iterations) on " "noisy data:\n" << cMo_estimated_RANSAC_parallel << std::endl; - std::cout << "Computation time: " << t_RANSAC_parallel << " ms" - << std::endl; + std::cout << "Computation time: " << t_RANSAC_parallel << " ms" << std::endl; - r_RANSAC_estimated_parallel = - ground_truth_pose.computeResidual(cMo_estimated_RANSAC_parallel); - std::cout << "Corresponding residual (1000 iterations): " - << r_RANSAC_estimated_parallel << std::endl; + r_RANSAC_estimated_parallel = ground_truth_pose.computeResidual(cMo_estimated_RANSAC_parallel); + std::cout << "Corresponding residual (1000 iterations): " << r_RANSAC_estimated_parallel << std::endl; vpHomogeneousMatrix cMo_estimated_RANSAC_parallel2; double t_RANSAC_parallel2 = vpTime::measureTimeMs(); - pose_ransac_parallel2.computePose(vpPose::RANSAC, - cMo_estimated_RANSAC_parallel2); + pose_ransac_parallel2.computePose(vpPose::RANSAC, cMo_estimated_RANSAC_parallel2); t_RANSAC_parallel2 = vpTime::measureTimeMs() - t_RANSAC_parallel2; - std::cout << "\ncMo estimated with parallel RANSAC (" << ransac_iterations - << " iterations) on noisy data:\n" + std::cout << "\ncMo estimated with parallel RANSAC (" << ransac_iterations << " iterations) on noisy data:\n" << cMo_estimated_RANSAC_parallel2 << std::endl; - std::cout << "Computation time: " << t_RANSAC_parallel2 << " ms" - << std::endl; + std::cout << "Computation time: " << t_RANSAC_parallel2 << " ms" << std::endl; - double r_RANSAC_estimated_parallel2 = - ground_truth_pose.computeResidual(cMo_estimated_RANSAC_parallel2); - std::cout << "Corresponding residual (" << ransac_iterations - << " iterations): " << r_RANSAC_estimated_parallel2 + double r_RANSAC_estimated_parallel2 = ground_truth_pose.computeResidual(cMo_estimated_RANSAC_parallel2); + std::cout << "Corresponding residual (" << ransac_iterations << " iterations): " << r_RANSAC_estimated_parallel2 << std::endl; } #endif // Check inlier index - std::vector vectorOfFoundInlierIndex = - pose_ransac.getRansacInlierIndex(); - int nbInlierIndexOk = - checkInlierIndex(vectorOfFoundInlierIndex, vectorOfOutlierFlags); + std::vector vectorOfFoundInlierIndex = pose_ransac.getRansacInlierIndex(); + int nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex, vectorOfOutlierFlags); - int nbTrueInlierIndex = (int)std::count(vectorOfOutlierFlags.begin(), - vectorOfOutlierFlags.end(), false); - std::cout << "\nThere are " << nbInlierIndexOk << " true inliers found, " - << vectorOfFoundInlierIndex.size() << " inliers returned and " - << nbTrueInlierIndex << " true inliers." << std::endl; + int nbTrueInlierIndex = (int)std::count(vectorOfOutlierFlags.begin(), vectorOfOutlierFlags.end(), false); + std::cout << "\nThere are " << nbInlierIndexOk << " true inliers found, " << vectorOfFoundInlierIndex.size() + << " inliers returned and " << nbTrueInlierIndex << " true inliers." << std::endl; // Check inlier points returned - std::vector vectorOfFoundInlierPoints = - pose_ransac.getRansacInliers(); + std::vector vectorOfFoundInlierPoints = pose_ransac.getRansacInliers(); if (vectorOfFoundInlierPoints.size() != vectorOfFoundInlierIndex.size()) { std::cerr << "The number of inlier index is different with the number of " @@ -2076,37 +2019,28 @@ bool testRansac(const std::vector &bunnyModelPoints_original, << std::endl; return false; } - if (!checkInlierPoints(vectorOfFoundInlierPoints, vectorOfFoundInlierIndex, - bunnyModelPoints_noisy)) { + if (!checkInlierPoints(vectorOfFoundInlierPoints, vectorOfFoundInlierIndex, bunnyModelPoints_noisy)) { return false; } // Check for RANSAC with p=0.99, epsilon=0.4 // Check inlier index - std::cout << "\nCheck for RANSAC iterations: " << ransac_iterations - << std::endl; - std::vector vectorOfFoundInlierIndex_2 = - pose_ransac2.getRansacInlierIndex(); - nbInlierIndexOk = - checkInlierIndex(vectorOfFoundInlierIndex_2, vectorOfOutlierFlags); + std::cout << "\nCheck for RANSAC iterations: " << ransac_iterations << std::endl; + std::vector vectorOfFoundInlierIndex_2 = pose_ransac2.getRansacInlierIndex(); + nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_2, vectorOfOutlierFlags); - std::cout << "There are " << nbInlierIndexOk << " true inliers found, " - << vectorOfFoundInlierIndex_2.size() << " inliers returned and " - << nbTrueInlierIndex << " true inliers." << std::endl; + std::cout << "There are " << nbInlierIndexOk << " true inliers found, " << vectorOfFoundInlierIndex_2.size() + << " inliers returned and " << nbTrueInlierIndex << " true inliers." << std::endl; // Check inlier points returned - std::vector vectorOfFoundInlierPoints_2 = - pose_ransac2.getRansacInliers(); - if (vectorOfFoundInlierPoints_2.size() != - vectorOfFoundInlierIndex_2.size()) { + std::vector vectorOfFoundInlierPoints_2 = pose_ransac2.getRansacInliers(); + if (vectorOfFoundInlierPoints_2.size() != vectorOfFoundInlierIndex_2.size()) { std::cerr << "The number of inlier index is different with the number of " "inlier points !" << std::endl; return false; } - if (!checkInlierPoints(vectorOfFoundInlierPoints_2, - vectorOfFoundInlierIndex_2, - bunnyModelPoints_noisy)) { + if (!checkInlierPoints(vectorOfFoundInlierPoints_2, vectorOfFoundInlierIndex_2, bunnyModelPoints_noisy)) { return false; } @@ -2115,69 +2049,51 @@ bool testRansac(const std::vector &bunnyModelPoints_original, // Check for parallel RANSAC // Check inlier index std::cout << "\nCheck for parallel RANSAC (1000 iterations)" << std::endl; - std::vector vectorOfFoundInlierIndex_parallel = - pose_ransac_parallel.getRansacInlierIndex(); - nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_parallel, - vectorOfOutlierFlags); + std::vector vectorOfFoundInlierIndex_parallel = pose_ransac_parallel.getRansacInlierIndex(); + nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_parallel, vectorOfOutlierFlags); - std::cout << "There are " << nbInlierIndexOk << " true inliers found, " - << vectorOfFoundInlierIndex_parallel.size() - << " inliers returned and " << nbTrueInlierIndex - << " true inliers." << std::endl; + std::cout << "There are " << nbInlierIndexOk << " true inliers found, " << vectorOfFoundInlierIndex_parallel.size() + << " inliers returned and " << nbTrueInlierIndex << " true inliers." << std::endl; // Check inlier points returned - std::vector vectorOfFoundInlierPoints_parallel = - pose_ransac_parallel.getRansacInliers(); - if (vectorOfFoundInlierPoints_parallel.size() != - vectorOfFoundInlierIndex_parallel.size()) { + std::vector vectorOfFoundInlierPoints_parallel = pose_ransac_parallel.getRansacInliers(); + if (vectorOfFoundInlierPoints_parallel.size() != vectorOfFoundInlierIndex_parallel.size()) { std::cerr << "The number of inlier index is different with the number " "of inlier points !" << std::endl; return false; } - if (!checkInlierPoints(vectorOfFoundInlierPoints_parallel, - vectorOfFoundInlierIndex_parallel, + if (!checkInlierPoints(vectorOfFoundInlierPoints_parallel, vectorOfFoundInlierIndex_parallel, bunnyModelPoints_noisy)) { return false; } // Check for parallel RANSAC 2 // Check inlier index - std::cout << "\nCheck for parallel RANSAC (" << ransac_iterations - << " iterations)" << std::endl; - std::vector vectorOfFoundInlierIndex_parallel2 = - pose_ransac_parallel2.getRansacInlierIndex(); - nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_parallel2, - vectorOfOutlierFlags); - - std::cout << "There are " << nbInlierIndexOk << " true inliers found, " - << vectorOfFoundInlierIndex_parallel2.size() - << " inliers returned and " << nbTrueInlierIndex - << " true inliers." << std::endl; + std::cout << "\nCheck for parallel RANSAC (" << ransac_iterations << " iterations)" << std::endl; + std::vector vectorOfFoundInlierIndex_parallel2 = pose_ransac_parallel2.getRansacInlierIndex(); + nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_parallel2, vectorOfOutlierFlags); + + std::cout << "There are " << nbInlierIndexOk << " true inliers found, " << vectorOfFoundInlierIndex_parallel2.size() + << " inliers returned and " << nbTrueInlierIndex << " true inliers." << std::endl; // Check inlier points returned - std::vector vectorOfFoundInlierPoints_parallel2 = - pose_ransac_parallel2.getRansacInliers(); - if (vectorOfFoundInlierPoints_parallel2.size() != - vectorOfFoundInlierIndex_parallel2.size()) { + std::vector vectorOfFoundInlierPoints_parallel2 = pose_ransac_parallel2.getRansacInliers(); + if (vectorOfFoundInlierPoints_parallel2.size() != vectorOfFoundInlierIndex_parallel2.size()) { std::cerr << "The number of inlier index is different with the number " "of inlier points !" << std::endl; return false; } - if (!checkInlierPoints(vectorOfFoundInlierPoints_parallel2, - vectorOfFoundInlierIndex_parallel2, + if (!checkInlierPoints(vectorOfFoundInlierPoints_parallel2, vectorOfFoundInlierIndex_parallel2, bunnyModelPoints_noisy)) { return false; } } #endif - if (r_RANSAC_estimated > - threshold /*|| r_RANSAC_estimated_2 > threshold*/) { - std::cerr - << "The pose estimated with the RANSAC method is badly estimated!" - << std::endl; + if (r_RANSAC_estimated > threshold /*|| r_RANSAC_estimated_2 > threshold*/) { + std::cerr << "The pose estimated with the RANSAC method is badly estimated!" << std::endl; std::cerr << "r_RANSAC_estimated=" << r_RANSAC_estimated << std::endl; std::cerr << "threshold=" << threshold << std::endl; return false; @@ -2188,16 +2104,13 @@ bool testRansac(const std::vector &bunnyModelPoints_original, std::cerr << "The pose estimated with the parallel RANSAC method is " "badly estimated!" << std::endl; - std::cerr << "r_RANSAC_estimated_parallel=" - << r_RANSAC_estimated_parallel << std::endl; + std::cerr << "r_RANSAC_estimated_parallel=" << r_RANSAC_estimated_parallel << std::endl; std::cerr << "threshold=" << threshold << std::endl; return false; } } #endif - std::cout - << "The pose estimated with the RANSAC method is well estimated!" - << std::endl; + std::cout << "The pose estimated with the RANSAC method is well estimated!" << std::endl; } return true; @@ -2228,12 +2141,10 @@ int main() std::cout << std::endl; for (int cpt1 = 0; cpt1 < 7; cpt1++) { - std::cout << std::setfill(' ') << std::setw(6) << sample_sizes[cpt1] - << "|"; + std::cout << std::setfill(' ') << std::setw(6) << sample_sizes[cpt1] << "|"; for (int cpt2 = 0; cpt2 < 7; cpt2++) { - int ransac_iters = vpPose::computeRansacIterations( - 0.99, epsilon[cpt2], sample_sizes[cpt1], -1); + int ransac_iters = vpPose::computeRansacIterations(0.99, epsilon[cpt2], sample_sizes[cpt1], -1); std::cout << std::setfill(' ') << std::setw(6) << ransac_iters; } std::cout << std::endl; @@ -2242,10 +2153,8 @@ int main() try { size_t model_sizes[] = {10, 20, 50, 100, 200, 500, 1000, 0, 0}; - bool duplicates[] = {false, false, false, false, false, - false, false, false, true}; - bool degenerates[] = {false, false, false, false, false, - false, true, true, true}; + bool duplicates[] = {false, false, false, false, false, false, false, false, true}; + bool degenerates[] = {false, false, false, false, false, false, true, true, true}; int nb = 9; std::vector bunnyModelPoints, bunnyModelPoints_noisy_original; @@ -2256,17 +2165,14 @@ int main() "===============" << std::endl; if (model_sizes[cpt] == 0) { - std::cout << "Test on " << bunnyModelPoints_noisy_original.size() - << " model points." << std::endl; + std::cout << "Test on " << bunnyModelPoints_noisy_original.size() << " model points." << std::endl; } else { - std::cout << "Test on " << model_sizes[cpt] << " model points." - << std::endl; + std::cout << "Test on " << model_sizes[cpt] << " model points." << std::endl; } - std::cout << "Test duplicate: " << duplicates[cpt] - << " ; Test degenerate: " << degenerates[cpt] << std::endl; + std::cout << "Test duplicate: " << duplicates[cpt] << " ; Test degenerate: " << degenerates[cpt] << std::endl; - if (!testRansac(bunnyModelPoints, bunnyModelPoints_noisy_original, - model_sizes[cpt], duplicates[cpt], degenerates[cpt])) { + if (!testRansac(bunnyModelPoints, bunnyModelPoints_noisy_original, model_sizes[cpt], duplicates[cpt], + degenerates[cpt])) { return EXIT_FAILURE; } } diff --git a/modules/visual_features/include/visp3/visual_features/vpBasicFeature.h b/modules/visual_features/include/visp3/visual_features/vpBasicFeature.h index 54f7509fd5..150e36c6e3 100644 --- a/modules/visual_features/include/visp3/visual_features/vpBasicFeature.h +++ b/modules/visual_features/include/visp3/visual_features/vpBasicFeature.h @@ -109,19 +109,14 @@ class VISP_EXPORT vpBasicFeature /*! Return the dimension of the feature vector \f$\bf s\f$. */ unsigned int dimension_s() { return dim_s; } - virtual void display(const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color = vpColor::green, - unsigned int thickness = 1) const = 0; - virtual void display(const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color = vpColor::green, + virtual void display(const vpCameraParameters &cam, const vpImage &I, + const vpColor &color = vpColor::green, unsigned int thickness = 1) const = 0; + virtual void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const = 0; virtual void init() = 0; - virtual vpColVector error(const vpBasicFeature &s_star, - const unsigned int select = FEATURE_ALL); + virtual vpColVector error(const vpBasicFeature &s_star, const unsigned int select = FEATURE_ALL); // Get the feature vector. vpColVector get_s(unsigned int select = FEATURE_ALL) const; @@ -132,10 +127,7 @@ class VISP_EXPORT vpBasicFeature //! Compute the interaction matrix from a subset of the possible features. virtual vpMatrix interaction(const unsigned int select = FEATURE_ALL) = 0; //! Return element \e i in the state vector (usage : x = s[i] ) - virtual inline double operator[](const unsigned int i) const - { - return s[i]; - } + virtual inline double operator[](const unsigned int i) const { return s[i]; } vpBasicFeature &operator=(const vpBasicFeature &f); //! Print the name of the feature. virtual void print(const unsigned int select = FEATURE_ALL) const = 0; diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureBuilder.h b/modules/visual_features/include/visp3/visual_features/vpFeatureBuilder.h index 64c3f7ae8b..b09bc5b82e 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureBuilder.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureBuilder.h @@ -94,40 +94,31 @@ 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); + 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 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(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); + 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, 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); + 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 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); + 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); @@ -137,23 +128,19 @@ class VISP_EXPORT vpFeatureBuilder 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); + 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); + 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); + static void create(vpFeatureEllipse &s, const vpCameraParameters &cam, const vpMeEllipse &d); #endif /*! @@ -165,14 +152,12 @@ class VISP_EXPORT vpFeatureBuilder 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); + 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); + 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 index 33357eaff2..ae9e49d549 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureDepth.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureDepth.h @@ -149,9 +149,10 @@ int main() // 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); + // 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 */ @@ -179,18 +180,14 @@ class VISP_EXPORT vpFeatureDepth : public vpBasicFeature section Set coordinates */ - void buildFrom(const double x, const double y, const double Z, - const double LogZoverZstar); + void buildFrom(const double x, const double y, const double Z, const double LogZoverZstar); - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; vpFeatureDepth *duplicate() const; - vpColVector error(const vpBasicFeature &s_star, - const unsigned int select = FEATURE_ALL); + vpColVector error(const vpBasicFeature &s_star, const unsigned int select = FEATURE_ALL); double get_x() const; @@ -212,8 +209,7 @@ class VISP_EXPORT vpFeatureDepth : public vpBasicFeature void set_LogZoverZstar(const double LogZoverZstar); - void set_xyZLogZoverZstar(const double x, const double y, const double Z, - const double logZZs); + void set_xyZLogZoverZstar(const double x, const double y, const double Z, const double logZZs); }; #endif diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureEllipse.h b/modules/visual_features/include/visp3/visual_features/vpFeatureEllipse.h index 01166f7d78..44e702b23b 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureEllipse.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureEllipse.h @@ -76,29 +76,23 @@ class VISP_EXPORT vpFeatureEllipse : public vpBasicFeature \section Set coordinates */ //! basic constructor - vpFeatureEllipse(const double x, const double y, const double mu20, - const double mu11, const double mu02); + 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 display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + 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 display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; //! Feature duplication vpFeatureEllipse *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); + 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); diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureException.h b/modules/visual_features/include/visp3/visual_features/vpFeatureException.h index cc0e4990f4..23e329c445 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureException.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureException.h @@ -93,11 +93,7 @@ class VISP_EXPORT vpFeatureException : public vpException setMessage(format, args); va_end(args); } - vpFeatureException(const int id, const std::string &msg) - : vpException(id, msg) - { - ; - } + vpFeatureException(const int id, const std::string &msg) : vpException(id, msg) { ; } explicit vpFeatureException(const int id) : vpException(id) { ; } }; diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLine.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLine.h index 4ec549dbd3..83ed4303cc 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLine.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLine.h @@ -105,11 +105,9 @@ int main() //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. + //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 + //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; @@ -125,8 +123,7 @@ interaction matrix only 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 + //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. @@ -167,9 +164,8 @@ int main() //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; + //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); @@ -177,18 +173,17 @@ frame : AX+BY+CZ+D=0. double Ad = 0; double Bd = 0; double Cd = 1; double Dd = //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 ! + //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 + // 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 @@ -216,19 +211,15 @@ class VISP_EXPORT vpFeatureLine : public vpBasicFeature // 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); + void buildFrom(const double rho, const double theta, const double A, const double B, const double C, const double D); - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; vpFeatureLine *duplicate() const; - vpColVector error(const vpBasicFeature &s_star, - 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) ; /*! @@ -247,8 +238,7 @@ class VISP_EXPORT vpFeatureLine : public vpBasicFeature void print(const unsigned int select = FEATURE_ALL) const; void setRhoTheta(const double rho, const double theta); - void setABCD(const double A, const double B, const double C, - const double D); + void setABCD(const double A, const double B, const double C, const double D); public: static unsigned int selectRho(); diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h index bf89394bb0..cb64bb4fff 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h @@ -102,17 +102,14 @@ class VISP_EXPORT vpFeatureLuminance : public vpBasicFeature void buildFrom(vpImage &I); - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; vpFeatureLuminance *duplicate() const; - vpColVector error(const vpBasicFeature &s_star, - const unsigned int select = FEATURE_ALL); + vpColVector error(const vpBasicFeature &s_star, const unsigned int select = FEATURE_ALL); void error(const vpBasicFeature &s_star, vpColVector &e); //! Compute the error between a visual features and zero vpColVector error(const unsigned int select = FEATURE_ALL); diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h index c7c7791e57..6136e38bc6 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h @@ -112,8 +112,11 @@ 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 #include - #include +vpFeatureMomentBasic). +\code +#include +#include +#include #include #include #include @@ -123,15 +126,15 @@ int main() vpPoint p; std::vector 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); + 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 + //////////////////////////////REFERENCE VALUES//////////////////////////////// + // Init object of order 3 because we need vpFeatureMomentBasic of order 2 which // implies third-order moment primitives + vpMomentObject obj(3); obj.setType(vpMomentObject::DISCRETE); // Discrete mode for object obj.fromVector(vec_p); @@ -143,8 +146,9 @@ of order 3 because we need vpFeatureMomentBasic fmb(mdb,0,0,1,NULL); - //update and compute the vpMoment BEFORE doing any operations with -vpFeatureMoment bm.update(obj); bm.compute(); + //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 @@ -197,12 +201,9 @@ class VISP_EXPORT vpFeatureMoment : public vpBasicFeature 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), + 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_), _name() { } @@ -213,11 +214,9 @@ class VISP_EXPORT vpFeatureMoment : public vpBasicFeature //@{ virtual void compute_interaction(void); vpBasicFeature *duplicate() const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; int getDimension(unsigned int select = FEATURE_ALL) const; @@ -240,8 +239,7 @@ class VISP_EXPORT vpFeatureMoment : public vpBasicFeature void update(double A, double B, double C); //@} - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpFeatureMoment &featM); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpFeatureMoment &featM); }; /*! @@ -265,9 +263,8 @@ 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) + 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; diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h index 55dc9ef16a..3a56c75f82 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h @@ -89,8 +89,7 @@ class VISP_EXPORT vpFeatureMomentAlpha : public vpFeatureMoment Feature database. */ - vpFeatureMomentAlpha(vpMomentDatabase &moments, double A, double B, - double C, + vpFeatureMomentAlpha(vpMomentDatabase &moments, double A, double B, double C, vpFeatureMomentDatabase *featureMoments = NULL) : vpFeatureMoment(moments, A, B, C, featureMoments, 1) { @@ -174,8 +173,7 @@ class VISP_EXPORT vpFeatureMomentAlpha : public vpFeatureMoment Feature database. */ - vpFeatureMomentAlpha(vpMomentDatabase &data_base, double A_, double B_, - double C_, + vpFeatureMomentAlpha(vpMomentDatabase &data_base, double A_, double B_, double C_, vpFeatureMomentDatabase *featureMoments = NULL) : vpFeatureMoment(data_base, A_, B_, C_, featureMoments, 1) { @@ -191,8 +189,7 @@ class VISP_EXPORT vpFeatureMomentAlpha : public vpFeatureMoment */ const char *name() const { return "vpFeatureMomentAlpha"; } - vpColVector error(const vpBasicFeature &s_star, - const unsigned int select = FEATURE_ALL); + 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 index 568a44fe1c..caf9127f2b 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h @@ -71,8 +71,7 @@ class VISP_EXPORT vpFeatureMomentArea : public vpFeatureMoment Feature database. */ - vpFeatureMomentArea(vpMomentDatabase &data_base, double A_, double B_, - double C_, + vpFeatureMomentArea(vpMomentDatabase &data_base, double A_, double B_, double C_, vpFeatureMomentDatabase *featureMoments = NULL) : vpFeatureMoment(data_base, A_, B_, C_, featureMoments, 1) { diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h index d5098623ba..2271f44dcb 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h @@ -93,9 +93,8 @@ class VISP_EXPORT vpFeatureMomentAreaNormalized : public vpFeatureMoment Feature database. */ - vpFeatureMomentAreaNormalized( - vpMomentDatabase &database, double A_, double B_, double C_, - vpFeatureMomentDatabase *featureMoments = NULL) + vpFeatureMomentAreaNormalized(vpMomentDatabase &database, double A_, double B_, double C_, + vpFeatureMomentDatabase *featureMoments = NULL) : vpFeatureMoment(database, A_, B_, C_, featureMoments, 1) { } @@ -189,9 +188,8 @@ class VISP_EXPORT vpFeatureMomentAreaNormalized : public vpFeatureMoment Feature database. */ - vpFeatureMomentAreaNormalized( - vpMomentDatabase &data_base, double A_, double B_, double C_, - vpFeatureMomentDatabase *featureMoments = NULL) + vpFeatureMomentAreaNormalized(vpMomentDatabase &data_base, double A_, double B_, double C_, + vpFeatureMomentDatabase *featureMoments = NULL) : vpFeatureMoment(data_base, A_, B_, C_, featureMoments, 1) { } diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h index fa547f740a..059e41ec99 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h @@ -56,11 +56,11 @@ class vpMomentDatabase; 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 + \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 @@ -81,8 +81,7 @@ class VISP_EXPORT vpFeatureMomentBasic : public vpFeatureMoment unsigned int order; public: - vpFeatureMomentBasic(vpMomentDatabase &moments, double A, double B, - double C, + vpFeatureMomentBasic(vpMomentDatabase &moments, double A, double B, double C, vpFeatureMomentDatabase *featureMoments = NULL); void compute_interaction(); @@ -90,13 +89,11 @@ class VISP_EXPORT vpFeatureMomentBasic : public vpFeatureMoment /* Add function due to pure virtual definition in vpBasicFeature.h */ vpMatrix interaction(const unsigned int /* select = FEATURE_ALL */) { - throw vpException(vpException::functionNotImplementedError, - "Not implemented!"); + throw vpException(vpException::functionNotImplementedError, "Not implemented!"); } #endif - vpMatrix interaction(unsigned int select_one, - unsigned int select_two) const; + vpMatrix interaction(unsigned int select_one, unsigned int select_two) const; /*! Associated moment name. */ diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h index 5e0ec70cbe..9f1f34337f 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h @@ -105,8 +105,7 @@ class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment Feature database. */ - vpFeatureMomentCInvariant(vpMomentDatabase &moments, double A, double B, - double C, + vpFeatureMomentCInvariant(vpMomentDatabase &moments, double A, double B, double C, vpFeatureMomentDatabase *featureMoments = NULL) : vpFeatureMoment(moments, A, B, C, featureMoments, 16) { @@ -246,8 +245,7 @@ class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment Feature database. */ - vpFeatureMomentCInvariant(vpMomentDatabase &data_base, double A_, double B_, - double C_, + vpFeatureMomentCInvariant(vpMomentDatabase &data_base, double A_, double B_, double C_, vpFeatureMomentDatabase *featureMoments = NULL) : vpFeatureMoment(data_base, A_, B_, C_, featureMoments, 16), LI(16) { @@ -325,8 +323,7 @@ class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment */ void printLsofInvariants(std::ostream &os) const; - friend VISP_EXPORT std::ostream & - operator<<(std::ostream &os, const vpFeatureMomentCInvariant &featcinv); + 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 index 7f213fdbb6..2a2f1cd3e7 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCentered.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCentered.h @@ -81,15 +81,12 @@ class VISP_EXPORT vpFeatureMomentCentered : public vpFeatureMoment /*! 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, + 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, + vpFeatureMomentCentered(vpMomentDatabase &moments, double A, double B, double C, vpFeatureMomentDatabase *featureMoments = NULL); void compute_interaction(); @@ -97,8 +94,7 @@ class VISP_EXPORT vpFeatureMomentCentered : public vpFeatureMoment /* Add function due to pure virtual definition in vpBasicFeature.h */ vpMatrix interaction(const unsigned int /* select = FEATURE_ALL */) { - throw vpException(vpException::functionNotImplementedError, - "Not implemented!"); + throw vpException(vpException::functionNotImplementedError, "Not implemented!"); } #endif /*! @@ -107,8 +103,7 @@ class VISP_EXPORT vpFeatureMomentCentered : public vpFeatureMoment \param select_two : second index (j) \return Interaction matrix corresponding to the moment */ - vpMatrix interaction(unsigned int select_one, - unsigned int select_two) const; + vpMatrix interaction(unsigned int select_one, unsigned int select_two) const; /*! associated moment name @@ -119,8 +114,7 @@ class VISP_EXPORT vpFeatureMomentCentered : public vpFeatureMoment */ const char *name() const { return "vpFeatureMomentCentered"; } - friend VISP_EXPORT std::ostream & - operator<<(std::ostream &os, const vpFeatureMomentCentered &v); + 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 index 1d6d9aec48..366bdbd587 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCommon.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCommon.h @@ -96,66 +96,66 @@ However, some shortcuts to retrieve the features are provided. pre-filled database. The following code demonstrates the construction of a 6x6 interaction matrix -as described in [1]. \code - -#include -#include -#include +as described in [1]. +\code #include +#include #include #include #include #include +#include +#include int main() { // Define source polygon vpPoint p; - std::vector vec_p; // vector that contains the vertices of the -contour polygon + std::vector 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) + 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); - 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(); + 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) + 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.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 + 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.); + 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.); + vpMomentCommon mdb_dst(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),1.); //Init classic features vpFeatureMomentCommon fmdb_dst(mdb_dst); @@ -181,8 +181,7 @@ mdb_dst(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentComm 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.getFeatureCInvariant(),fmdb_dst.getFeatureCInvariant(),(1 << 3) | (1 << 5)); task.addFeature(fmdb_src.getFeatureAlpha(),fmdb_dst.getFeatureAlpha()); task.setLambda(1) ; @@ -237,8 +236,7 @@ class VISP_EXPORT vpFeatureMomentCommon : public vpFeatureMomentDatabase vpFeatureMomentArea feature_moment_area; public: - vpFeatureMomentCommon(vpMomentDatabase &moments, double A = 0.0, - double B = 0.0, double C = 1.0); + 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. @@ -261,17 +259,11 @@ class VISP_EXPORT vpFeatureMomentCommon : public vpFeatureMomentDatabase /*! Returns non-symmetric invariants. */ - vpFeatureMomentCInvariant &getFeatureCInvariant() - { - return featureCInvariant; - } + vpFeatureMomentCInvariant &getFeatureCInvariant() { return featureCInvariant; } /*! Returns normalized gravity center. */ - vpFeatureMomentGravityCenterNormalized &getFeatureGravityNormalized() - { - return featureGravityNormalized; - } + vpFeatureMomentGravityCenterNormalized &getFeatureGravityNormalized() { return featureGravityNormalized; } /*! Returns the area */ @@ -279,10 +271,7 @@ class VISP_EXPORT vpFeatureMomentCommon : public vpFeatureMomentDatabase /*! Returns gravity center */ - vpFeatureMomentGravityCenter &getFeatureGravityCenter() - { - return featureGravity; - } + 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 index 646d0ba41a..cc6d8c5f9b 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h @@ -73,22 +73,25 @@ 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 - +between moment primitives and moment features: +\code +#include + +#include #include #include #include -#include +#include #include -#include +#include -#include -#include -#include #include +#include #include #include +#include +#include int main() { @@ -97,13 +100,13 @@ int main() vpPoint p; std::vector 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); + 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 + //////////////////////////////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); @@ -144,8 +147,9 @@ of order 6 because we are 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; + std::cout << fci.interaction(vpFeatureMomentCInvariant::selectC1()) << std::endl; + }catch(vpException &e){ + std::cout << e.getMessage() << std::endl; } return 0; @@ -156,14 +160,8 @@ 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 + bool operator()(const char *a, const char *b) const { return std::strcmp(a, b) < 0; } + char *operator=(const char *) { return NULL; } // Only to avoid a warning under Visual with /Wall flag }; std::map featureMomentsDataBase; void add(vpFeatureMoment &featureMoment, char *name); diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h index a8c60f3a87..3734c23fce 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h @@ -63,32 +63,31 @@ following methods: vpFeatureMomentGravityCenter::selectXg for \f$ L_{x_{g}} following code demonstrates a selection of \f$ L_{y_{g}} \f$: \code -#include -#include +#include #include -#include #include -#include +#include #include #include -#include #include #include +#include +#include +#include int main() { vpPoint p; std::vector 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); + 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 + //////////////////////////////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); @@ -110,10 +109,12 @@ vpFeatureMomentGravityCenter) which 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(); + //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 @@ -125,7 +126,6 @@ vpFeatureMomentBasic gc.update(obj); gc.compute(); return 0; } - \endcode This code produces the following output: @@ -138,8 +138,10 @@ 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: + 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. @@ -158,8 +160,7 @@ class VISP_EXPORT vpFeatureMomentGravityCenter : public vpFeatureMoment 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_, + vpFeatureMomentGravityCenter(vpMomentDatabase &database, double A_, double B_, double C_, vpFeatureMomentDatabase *featureMoments = NULL) : vpFeatureMoment(database, A_, B_, C_, featureMoments, 2) { @@ -207,9 +208,10 @@ class vpMomentDatabase; vpFeatureMomentGravityCenter::selectYg as follows: \code - task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenter::selectXg() - | vpFeatureMomentGravityCenter::selectYg()); \endcode This feature depends - on: + task.addFeature(db_src.getFeatureGravityNormalized(), db_dst.getFeatureGravityNormalized(), + vpFeatureMomentGravityCenter::selectXg() | vpFeatureMomentGravityCenter::selectYg()); + \endcode + This feature depends on: - vpMomentCentered - vpMomentGravityCenter @@ -229,8 +231,7 @@ class VISP_EXPORT vpFeatureMomentGravityCenter : public vpFeatureMoment 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_, + vpFeatureMomentGravityCenter(vpMomentDatabase &data_base, double A_, double B_, double C_, vpFeatureMomentDatabase *featureMoments = NULL) : vpFeatureMoment(data_base, A_, B_, C_, featureMoments, 2) { diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h index 86bb2fe3f0..2048203b28 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h @@ -66,8 +66,9 @@ class vpMomentDatabase; 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 + 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 @@ -86,8 +87,7 @@ class vpMomentDatabase; - vpFeatureMomentAreaNormalized */ -class VISP_EXPORT vpFeatureMomentGravityCenterNormalized - : public vpFeatureMoment +class VISP_EXPORT vpFeatureMomentGravityCenterNormalized : public vpFeatureMoment { public: /*! @@ -102,9 +102,8 @@ class VISP_EXPORT vpFeatureMomentGravityCenterNormalized Feature database. */ - vpFeatureMomentGravityCenterNormalized( - vpMomentDatabase &database, double A_, double B_, double C_, - vpFeatureMomentDatabase *featureMoments = NULL) + vpFeatureMomentGravityCenterNormalized(vpMomentDatabase &database, double A_, double B_, double C_, + vpFeatureMomentDatabase *featureMoments = NULL) : vpFeatureMoment(database, A_, B_, C_, featureMoments, 2) { } @@ -116,10 +115,7 @@ class VISP_EXPORT vpFeatureMomentGravityCenterNormalized /*! feature name */ - const char *name() const - { - return "vpFeatureMomentGravityCenterNormalized"; - } + const char *name() const { return "vpFeatureMomentGravityCenterNormalized"; } /*! Shortcut selector for \f$x_n\f$. @@ -220,8 +216,10 @@ the following methods: vpFeatureMomentGravityCenterNormalized::selectXn for 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 + 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 @@ -240,8 +238,7 @@ and 3 in discrete mode. - vpFeatureMomentAreaNormalized */ -class VISP_EXPORT vpFeatureMomentGravityCenterNormalized - : public vpFeatureMoment +class VISP_EXPORT vpFeatureMomentGravityCenterNormalized : public vpFeatureMoment { public: /*! @@ -256,9 +253,8 @@ class VISP_EXPORT vpFeatureMomentGravityCenterNormalized Feature database. */ - vpFeatureMomentGravityCenterNormalized( - vpMomentDatabase &data_base, double A_, double B_, double C_, - vpFeatureMomentDatabase *featureMoments = NULL) + vpFeatureMomentGravityCenterNormalized(vpMomentDatabase &data_base, double A_, double B_, double C_, + vpFeatureMomentDatabase *featureMoments = NULL) : vpFeatureMoment(data_base, A_, B_, C_, featureMoments, 2) { } @@ -270,10 +266,7 @@ class VISP_EXPORT vpFeatureMomentGravityCenterNormalized /*! feature name */ - const char *name() const - { - return "vpFeatureMomentGravityCenterNormalized"; - } + const char *name() const { return "vpFeatureMomentGravityCenterNormalized"; } /*! Shortcut selector for \f$x_n\f$. diff --git a/modules/visual_features/include/visp3/visual_features/vpFeaturePoint.h b/modules/visual_features/include/visp3/visual_features/vpFeaturePoint.h index 352a6709a2..4f957baf03 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeaturePoint.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeaturePoint.h @@ -110,8 +110,7 @@ int main() 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 + //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. @@ -167,8 +166,7 @@ int main() // 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 + // 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 @@ -194,17 +192,14 @@ class VISP_EXPORT vpFeaturePoint : public vpBasicFeature void buildFrom(const double x, const double y, const double Z); - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; vpFeaturePoint *duplicate() const; - vpColVector error(const vpBasicFeature &s_star, - const unsigned int select = FEATURE_ALL); + 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); diff --git a/modules/visual_features/include/visp3/visual_features/vpFeaturePoint3D.h b/modules/visual_features/include/visp3/visual_features/vpFeaturePoint3D.h index 4132321244..a4b6b0f299 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeaturePoint3D.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeaturePoint3D.h @@ -113,13 +113,12 @@ int main() 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 + 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(); + 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; @@ -141,8 +140,7 @@ in the camera frame: s=(X,Y,Z) s.print(); // 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 + 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); @@ -160,8 +158,8 @@ frame cP = cMo * oP \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 + 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 @@ -183,13 +181,12 @@ int main() 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 + 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(); + 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; @@ -227,11 +224,9 @@ class VISP_EXPORT vpFeaturePoint3D : public vpBasicFeature // set the point XY and Z-coordinates void buildFrom(const double X, const double Y, const double Z); - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; // feature duplication @@ -239,8 +234,7 @@ class VISP_EXPORT vpFeaturePoint3D : public vpBasicFeature // 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); + vpColVector error(const vpBasicFeature &s_star, const unsigned int select = FEATURE_ALL); // get the point X-coordinates double get_X() const; diff --git a/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h b/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h index 1b1b880bd5..be8c0b8151 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h @@ -221,8 +221,8 @@ int main() \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()); + // 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 @@ -249,8 +249,8 @@ int main() // 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); + // Compute the error vector (s-s*) for the point feature with polar coordinates + s.error(s_star); return 0; } @@ -272,11 +272,9 @@ class VISP_EXPORT vpFeaturePointPolar : public vpBasicFeature void buildFrom(const double rho, const double theta, const double Z); - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; // feature duplication @@ -284,8 +282,7 @@ class VISP_EXPORT vpFeaturePointPolar : public vpBasicFeature // 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); + vpColVector error(const vpBasicFeature &s_star, const unsigned int select = FEATURE_ALL); // basic construction void init(); diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h b/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h index da352a2f8f..7359077510 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h @@ -79,21 +79,17 @@ class VISP_EXPORT vpFeatureSegment : public vpBasicFeature //! 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 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 &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &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); + vpColVector error(const vpBasicFeature &s_star, const unsigned int select = FEATURE_ALL); /*! Get the x coordinate of the segment center in the image plane. @@ -247,16 +243,14 @@ class VISP_EXPORT vpFeatureSegment : public vpBasicFeature vpERROR_TRACE("Point is behind the camera "); std::cout << "Z1 = " << Z1_ << std::endl; - throw(vpFeatureException(vpFeatureException::badInitializationError, - "Point Z1 is behind the camera ")); + 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")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z1 coordinates is null")); } flags[4] = true; @@ -282,16 +276,14 @@ class VISP_EXPORT vpFeatureSegment : public vpBasicFeature vpERROR_TRACE("Point Z2 is behind the camera "); std::cout << "Z2 = " << Z2_ << std::endl; - throw(vpFeatureException(vpFeatureException::badInitializationError, - "Point Z2 is behind the camera ")); + 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")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z2 coordinates is null")); } flags[5] = true; diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureThetaU.h b/modules/visual_features/include/visp3/visual_features/vpFeatureThetaU.h index e098ed2f79..e0df60cb4f 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureThetaU.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureThetaU.h @@ -166,8 +166,7 @@ int main() // Control loop for ( ; ; ) { - // ... cMcd need here to be initialized from for example a pose -estimation. + // ... cMcd need here to be initialized from for example a pose estimation. // Update the current ThetaU visual feature s.buildFrom(cMcd); @@ -186,8 +185,8 @@ estimation. \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 + 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 @@ -252,12 +251,9 @@ class VISP_EXPORT vpFeatureThetaU : public vpBasicFeature // Basic constructor. vpFeatureThetaU(); explicit vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r); - vpFeatureThetaU(vpThetaUVector &tu, - vpFeatureThetaURotationRepresentationType r); - vpFeatureThetaU(vpRotationMatrix &R, - vpFeatureThetaURotationRepresentationType r); - vpFeatureThetaU(vpHomogeneousMatrix &M, - vpFeatureThetaURotationRepresentationType r); + vpFeatureThetaU(vpThetaUVector &tu, vpFeatureThetaURotationRepresentationType r); + vpFeatureThetaU(vpRotationMatrix &R, vpFeatureThetaURotationRepresentationType r); + vpFeatureThetaU(vpHomogeneousMatrix &M, vpFeatureThetaURotationRepresentationType r); //! Destructor. Does nothing. virtual ~vpFeatureThetaU() {} @@ -267,11 +263,9 @@ class VISP_EXPORT vpFeatureThetaU : public vpBasicFeature // build from an homogeneous matrix void buildFrom(const vpHomogeneousMatrix &M); - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; //! Feature duplication. @@ -279,11 +273,9 @@ class VISP_EXPORT vpFeatureThetaU : public vpBasicFeature // 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); + vpColVector error(const vpBasicFeature &s_star, const unsigned int select = FEATURE_ALL); - vpFeatureThetaURotationRepresentationType - getFeatureThetaURotationType() const; + vpFeatureThetaURotationRepresentationType getFeatureThetaURotationType() const; double get_TUx() const; double get_TUy() const; @@ -300,8 +292,7 @@ class VISP_EXPORT vpFeatureThetaU : public vpBasicFeature void set_TUy(const double tu_y); void set_TUz(const double tu_z); - void setFeatureThetaURotationType( - const vpFeatureThetaURotationRepresentationType r); + void setFeatureThetaURotationType(const vpFeatureThetaURotationRepresentationType r); public: /* diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h b/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h index b0fa04f095..4894fb3d97 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h @@ -162,8 +162,7 @@ int main() // Control loop for ( ; ; ) { - // ... cdMc need here to be initialized from for example a pose -estimation. + // ... cdMc need here to be initialized from for example a pose estimation. // Update the current 3D translation visual feature s.buildFrom(cdMc); @@ -181,8 +180,8 @@ estimation. \code // Add the (tx,ty) subset features from 3D translation to the task - task.addFeature(s, vpFeatureTranslation::selectTx() | -vpFeatureTranslation::selectTy()); \endcode + 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 @@ -239,8 +238,7 @@ int main() // 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*) + s_star.buildFrom(cdMo); // Initialization of the desired feature s*=(tx*,ty*,tz*) vpHomogeneousMatrix cMo; // ... cMo need here to be computed. @@ -303,8 +301,7 @@ class VISP_EXPORT vpFeatureTranslation : public vpBasicFeature explicit vpFeatureTranslation(vpFeatureTranslationRepresentationType r); // constructor : build from an homogeneous matrix // cdMc is the displacement that the camera has to realize - vpFeatureTranslation(vpHomogeneousMatrix &f2Mf1, - vpFeatureTranslationRepresentationType r); + vpFeatureTranslation(vpHomogeneousMatrix &f2Mf1, vpFeatureTranslationRepresentationType r); //! Destructor. Does nothing. virtual ~vpFeatureTranslation() {} @@ -312,11 +309,9 @@ class VISP_EXPORT vpFeatureTranslation : public vpBasicFeature // cdMc is the displacement that the camera has to realize void buildFrom(const vpHomogeneousMatrix &f2Mf1); - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; //! Feature duplication @@ -324,8 +319,7 @@ class VISP_EXPORT vpFeatureTranslation : public vpBasicFeature // 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); + vpColVector error(const vpBasicFeature &s_star, const unsigned int select = FEATURE_ALL); vpFeatureTranslationRepresentationType getFeatureTranslationType() const; @@ -345,8 +339,7 @@ class VISP_EXPORT vpFeatureTranslation : public vpBasicFeature void set_Ty(const double t_y); void set_Tz(const double t_z); - void - setFeatureTranslationType(const vpFeatureTranslationRepresentationType r); + void setFeatureTranslationType(const vpFeatureTranslationRepresentationType r); // feature selection static unsigned int selectTx(); diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureVanishingPoint.h b/modules/visual_features/include/visp3/visual_features/vpFeatureVanishingPoint.h index 9c9a5ecb4d..2bef898bac 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureVanishingPoint.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureVanishingPoint.h @@ -81,11 +81,9 @@ class VISP_EXPORT vpFeatureVanishingPoint : public vpBasicFeature // void buildFrom(const vpPoint &p) ; void buildFrom(const double _x, const double _y); - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; //! feature duplication @@ -93,8 +91,7 @@ class VISP_EXPORT vpFeatureVanishingPoint : public vpBasicFeature //! 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); + 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); diff --git a/modules/visual_features/include/visp3/visual_features/vpGenericFeature.h b/modules/visual_features/include/visp3/visual_features/vpGenericFeature.h index 52b33968dc..c8fcc964b5 100644 --- a/modules/visual_features/include/visp3/visual_features/vpGenericFeature.h +++ b/modules/visual_features/include/visp3/visual_features/vpGenericFeature.h @@ -74,16 +74,18 @@ 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. + //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. @@ -130,15 +132,16 @@ 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. + //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. + //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. @@ -185,17 +188,14 @@ class VISP_EXPORT vpGenericFeature : public vpBasicFeature explicit vpGenericFeature(unsigned int dim); virtual ~vpGenericFeature(); - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; - void display(const vpCameraParameters &cam, const vpImage &I, - const vpColor &color = vpColor::green, + void display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color = vpColor::green, unsigned int thickness = 1) const; vpGenericFeature *duplicate() const; - vpColVector error(const vpBasicFeature &s_star, - 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); @@ -218,11 +218,7 @@ class VISP_EXPORT vpGenericFeature : public vpBasicFeature void set_s(const double s0, const double s1, const double s2); private: - typedef enum { - errorNotInitalized, - errorInitialized, - errorHasToBeUpdated - } vpGenericFeatureErrorType; + typedef enum { errorNotInitalized, errorInitialized, errorHasToBeUpdated } vpGenericFeatureErrorType; vpMatrix L; vpColVector err; diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderEllipse.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderEllipse.cpp index 7f3d38492b..5e36f3cee0 100644 --- a/modules/visual_features/src/feature-builder/vpFeatureBuilderEllipse.cpp +++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderEllipse.cpp @@ -114,8 +114,7 @@ void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpSphere &t) 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 d = vpMath::sqr(X0) + vpMath::sqr(Y0) + vpMath::sqr(Z0) - vpMath::sqr(R); double A = X0 / d; double B = Y0 / d; @@ -150,8 +149,7 @@ void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpSphere &t) \param t : The vpDot used to create the vpFeatureEllipse. */ -void vpFeatureBuilder::create(vpFeatureEllipse &s, - const vpCameraParameters &cam, const vpDot &t) +void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpCameraParameters &cam, const vpDot &t) { try { @@ -208,8 +206,7 @@ void vpFeatureBuilder::create(vpFeatureEllipse &s, \param t : The vpDot2 used to create the vpFeatureEllipse. */ -void vpFeatureBuilder::create(vpFeatureEllipse &s, - const vpCameraParameters &cam, const vpDot2 &t) +void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpCameraParameters &cam, const vpDot2 &t) { try { @@ -268,9 +265,7 @@ void vpFeatureBuilder::create(vpFeatureEllipse &s, \param t : The vpMeEllipse used to create the vpFeatureEllipse. */ -void vpFeatureBuilder::create(vpFeatureEllipse &s, - const vpCameraParameters &cam, - const vpMeEllipse &t) +void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpCameraParameters &cam, const vpMeEllipse &t) { try { diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp index 367c2efb33..29f4000c7d 100644 --- a/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp +++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp @@ -99,8 +99,7 @@ void vpFeatureBuilder::create(vpFeatureLine &s, const vpLine &t) \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) +void vpFeatureBuilder::create(vpFeatureLine &s, const vpCylinder &t, const int line) { try { @@ -114,8 +113,8 @@ void vpFeatureBuilder::create(vpFeatureLine &s, const vpCylinder &t, 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 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; @@ -193,8 +192,7 @@ void vpFeatureBuilder::create(vpFeatureLine &s, const vpCylinder &t, s.setABCD(A,B,C,D); \endcode */ -void vpFeatureBuilder::create(vpFeatureLine &s, const vpCameraParameters &cam, - const vpMeLine &t) +void vpFeatureBuilder::create(vpFeatureLine &s, const vpCameraParameters &cam, const vpMeLine &t) { try { double rhop = t.getRho(); diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp index 1007b0c128..81131e43b0 100644 --- a/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp +++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp @@ -90,8 +90,7 @@ s.set_Z(Z); \endcode */ -void vpFeatureBuilder::create(vpFeaturePoint &s, - const vpCameraParameters &cam, const vpDot &d) +void vpFeatureBuilder::create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d) { try { double x = 0, y = 0; @@ -151,8 +150,7 @@ void vpFeatureBuilder::create(vpFeaturePoint &s, s.set_Z(Z); \endcode */ -void vpFeatureBuilder::create(vpFeaturePoint &s, - const vpCameraParameters &cam, const vpDot2 &d) +void vpFeatureBuilder::create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot2 &d) { try { double x = 0, y = 0; @@ -213,9 +211,7 @@ void vpFeatureBuilder::create(vpFeaturePoint &s, s.set_Z(Z); \endcode */ -void vpFeatureBuilder::create(vpFeaturePoint &s, - const vpCameraParameters &cam, - const vpImagePoint &ip) +void vpFeatureBuilder::create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpImagePoint &ip) { try { double x = 0, y = 0; @@ -256,16 +252,14 @@ void vpFeatureBuilder::create(vpFeaturePoint &s, const vpPoint &p) vpERROR_TRACE("Point is behind the camera "); std::cout << "Z = " << s.get_Z() << std::endl; - throw(vpFeatureException(vpFeatureException::badInitializationError, - "Point is behind the camera ")); + 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")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null")); } } catch (...) { @@ -305,9 +299,7 @@ void vpFeatureBuilder::create(vpFeaturePoint &s, const vpPoint &p) \param p : The vpPoint used to create the vpFeaturePoint. */ -void vpFeatureBuilder::create(vpFeaturePoint &s, - const vpCameraParameters &goodCam, - const vpCameraParameters &wrongCam, +void vpFeatureBuilder::create(vpFeaturePoint &s, const vpCameraParameters &goodCam, const vpCameraParameters &wrongCam, const vpPoint &p) { try { diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp index e45b51a7b5..966cad8ea4 100644 --- a/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp +++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp @@ -99,8 +99,7 @@ \endcode */ -void vpFeatureBuilder::create(vpFeaturePointPolar &s, - const vpCameraParameters &cam, const vpDot &dot) +void vpFeatureBuilder::create(vpFeaturePointPolar &s, const vpCameraParameters &cam, const vpDot &dot) { try { double x = 0, y = 0; @@ -172,9 +171,7 @@ void vpFeatureBuilder::create(vpFeaturePointPolar &s, \endcode */ -void vpFeatureBuilder::create(vpFeaturePointPolar &s, - const vpCameraParameters &cam, - const vpDot2 &dot) +void vpFeatureBuilder::create(vpFeaturePointPolar &s, const vpCameraParameters &cam, const vpDot2 &dot) { try { double x = 0, y = 0; @@ -245,9 +242,7 @@ void vpFeatureBuilder::create(vpFeaturePointPolar &s, \endcode */ -void vpFeatureBuilder::create(vpFeaturePointPolar &s, - const vpCameraParameters &cam, - const vpImagePoint &iP) +void vpFeatureBuilder::create(vpFeaturePointPolar &s, const vpCameraParameters &cam, const vpImagePoint &iP) { try { double x = 0, y = 0; @@ -302,16 +297,14 @@ void vpFeatureBuilder::create(vpFeaturePointPolar &s, const vpPoint &p) vpERROR_TRACE("Point is behind the camera "); std::cout << "Z = " << s.get_Z() << std::endl; - throw(vpFeatureException(vpFeatureException::badInitializationError, - "Point is behind the camera ")); + 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")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null")); } } catch (...) { @@ -353,10 +346,8 @@ void vpFeatureBuilder::create(vpFeaturePointPolar &s, const vpPoint &p) 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) +void vpFeatureBuilder::create(vpFeaturePointPolar &s, const vpCameraParameters &goodCam, + const vpCameraParameters &wrongCam, const vpPoint &p) { try { double x = p.get_x(); diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderSegment.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderSegment.cpp index 36bca8374a..b03a5322bd 100644 --- a/modules/visual_features/src/feature-builder/vpFeatureBuilderSegment.cpp +++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderSegment.cpp @@ -57,9 +57,7 @@ segment. */ -void vpFeatureBuilder::create(vpFeatureSegment &s, - const vpCameraParameters &cam, const vpDot &d1, - const vpDot &d2) +void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpDot &d1, const vpDot &d2) { double x1 = 0, y1 = 0, x2 = 0, y2 = 0; @@ -93,9 +91,7 @@ void vpFeatureBuilder::create(vpFeatureSegment &s, segment. */ -void vpFeatureBuilder::create(vpFeatureSegment &s, - const vpCameraParameters &cam, const vpDot2 &d1, - const vpDot2 &d2) +void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpDot2 &d1, const vpDot2 &d2) { double x1 = 0, y1 = 0, x2 = 0, y2 = 0; @@ -130,9 +126,7 @@ void vpFeatureBuilder::create(vpFeatureSegment &s, the second point of the segment. */ -void vpFeatureBuilder::create(vpFeatureSegment &s, - const vpCameraParameters &cam, - const vpImagePoint &ip1, +void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpImagePoint &ip1, const vpImagePoint &ip2) { double x1 = 0, y1 = 0, x2 = 0, y2 = 0; diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderVanishingPoint.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderVanishingPoint.cpp index 2c3379919f..5a9bff334c 100644 --- a/modules/visual_features/src/feature-builder/vpFeatureBuilderVanishingPoint.cpp +++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderVanishingPoint.cpp @@ -78,9 +78,7 @@ void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpPoint &t) \param L2 : The second vpFeatureLine. */ -void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, - const vpFeatureLine &L1, - const vpFeatureLine &L2) +void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpFeatureLine &L1, const vpFeatureLine &L2) { double rho_l; double rho_r; @@ -103,15 +101,13 @@ void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, double x, y; double min = 0.0001; - if (fabs(theta_r - theta_l) < min || - fabs(fabs(theta_r - theta_l) - M_PI) < min || + 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")); + 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); @@ -134,8 +130,7 @@ void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, \param L2 : The second vpLine. */ -void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpLine &L1, - const vpLine &L2) +void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpLine &L1, const vpLine &L2) { vpFeatureLine l1, l2; vpFeatureBuilder::create(l1, L1); diff --git a/modules/visual_features/src/visual-feature/vpBasicFeature.cpp b/modules/visual_features/src/visual-feature/vpBasicFeature.cpp index 2225070764..bd1a73ca31 100644 --- a/modules/visual_features/src/visual-feature/vpBasicFeature.cpp +++ b/modules/visual_features/src/visual-feature/vpBasicFeature.cpp @@ -40,17 +40,14 @@ #include 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)}; + (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 @@ -59,11 +56,7 @@ const unsigned int vpBasicFeature::FEATURE_LINE[32] = { /*! Default constructor. */ -vpBasicFeature::vpBasicFeature() - : s(), dim_s(0), flags(NULL), nbParameters(0), - deallocate(vpBasicFeature::user) -{ -} +vpBasicFeature::vpBasicFeature() : s(), dim_s(0), flags(NULL), nbParameters(0), deallocate(vpBasicFeature::user) {} /*! Destructor that free allocated memory. @@ -80,8 +73,7 @@ vpBasicFeature::~vpBasicFeature() Copy constructor. */ vpBasicFeature::vpBasicFeature(const vpBasicFeature &f) - : s(), dim_s(0), flags(NULL), nbParameters(0), - deallocate(vpBasicFeature::user) + : s(), dim_s(0), flags(NULL), nbParameters(0), deallocate(vpBasicFeature::user) { *this = f; } @@ -156,8 +148,7 @@ void vpBasicFeature::setFlags() //! 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 vpBasicFeature::error(const vpBasicFeature &s_star, const unsigned int select) { vpColVector e(0), eLine(1); if (dim_s <= 31) { diff --git a/modules/visual_features/src/visual-feature/vpFeatureDepth.cpp b/modules/visual_features/src/visual-feature/vpFeatureDepth.cpp index a8bef6679a..75de3496fe 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureDepth.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureDepth.cpp @@ -99,10 +99,7 @@ vpFeatureDepth::vpFeatureDepth() : x(0), y(0), Z(1.) { init(); } \param LogZoverZstar : \f$ log(\frac{Z}{Z^*}) \f$ value to set. */ -void vpFeatureDepth::set_LogZoverZstar(const double LogZoverZstar) -{ - s[0] = LogZoverZstar; -} +void vpFeatureDepth::set_LogZoverZstar(const double LogZoverZstar) { s[0] = LogZoverZstar; } /*! Get the value of \f$ log(\frac{Z}{Z^*}) \f$ which represents the logarithm @@ -184,9 +181,7 @@ double vpFeatureDepth::get_Z() const { return Z; } \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) +void vpFeatureDepth::set_xyZLogZoverZstar(const double x_, const double y_, const double Z_, const double LogZoverZstar) { set_x(x_); set_y(y_); @@ -212,7 +207,8 @@ void vpFeatureDepth::set_xyZLogZoverZstar(const double x_, const double y_, \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 + 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 @@ -259,16 +255,14 @@ vpMatrix vpFeatureDepth::interaction(const unsigned int select) vpERROR_TRACE("Point is behind the camera "); std::cout << "Z = " << Z_ << std::endl; - throw(vpFeatureException(vpFeatureException::badInitializationError, - "Point is behind the camera ")); + 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")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null")); } if (FEATURE_ALL & select) { @@ -320,14 +314,12 @@ vpMatrix vpFeatureDepth::interaction(const unsigned int select) s.error(s_star); \endcode */ -vpColVector vpFeatureDepth::error(const vpBasicFeature &s_star, - const unsigned int select) +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 !")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "s* should be zero !")); } vpColVector e(1); @@ -377,8 +369,7 @@ void vpFeatureDepth::print(const unsigned int select) const \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) +void vpFeatureDepth::buildFrom(const double x_, const double y_, const double Z_, const double LogZoverZstar) { s[0] = LogZoverZstar; @@ -391,16 +382,14 @@ void vpFeatureDepth::buildFrom(const double x_, const double y_, vpERROR_TRACE("Point is behind the camera "); std::cout << "Z = " << Z << std::endl; - throw(vpFeatureException(vpFeatureException::badInitializationError, - "Point is behind the camera ")); + 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")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null")); } for (unsigned int i = 0; i < nbParameters; i++) @@ -428,10 +417,8 @@ vpFeatureDepth *vpFeatureDepth::duplicate() const Not implemented. */ -void vpFeatureDepth::display(const vpCameraParameters & /* cam */, - const vpImage & /* I */, - const vpColor & /* color */, - unsigned int /* thickness */) const +void vpFeatureDepth::display(const vpCameraParameters & /* cam */, const vpImage & /* I */, + const vpColor & /* color */, unsigned int /* thickness */) const { static int firsttime = 0; @@ -447,10 +434,8 @@ void vpFeatureDepth::display(const vpCameraParameters & /* cam */, Not implemented. */ -void vpFeatureDepth::display(const vpCameraParameters & /* cam */, - const vpImage & /* I */, - const vpColor & /* color */, - unsigned int /* thickness */) const +void vpFeatureDepth::display(const vpCameraParameters & /* cam */, const vpImage & /* I */, + const vpColor & /* color */, unsigned int /* thickness */) const { static int firsttime = 0; diff --git a/modules/visual_features/src/visual-feature/vpFeatureEllipse.cpp b/modules/visual_features/src/visual-feature/vpFeatureEllipse.cpp index ae1995e30c..3fc2e99817 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureEllipse.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureEllipse.cpp @@ -223,8 +223,7 @@ vpMatrix vpFeatureEllipse::interaction(const unsigned int select) //! 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 vpFeatureEllipse::error(const vpBasicFeature &s_star, const unsigned int select) { vpColVector e(0); @@ -286,8 +285,7 @@ void vpFeatureEllipse::print(const unsigned int select) const 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, +void vpFeatureEllipse::buildFrom(const double x, const double y, const double mu20, const double mu11, const double mu02) { @@ -301,10 +299,8 @@ void vpFeatureEllipse::buildFrom(const double x, const double y, 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) +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; @@ -350,8 +346,7 @@ void vpFeatureEllipse::setABC(const double a, const double b, const double c) flags[i] = true; } -void vpFeatureEllipse::setMu(const double mu20, const double mu11, - const double mu02) +void vpFeatureEllipse::setMu(const double mu20, const double mu11, const double mu02) { s[2] = mu20; @@ -370,9 +365,7 @@ void vpFeatureEllipse::setMu(const double mu20, const double mu11, \param color : Color used to display the feature. \param thickness : Thickness of the feature representation. */ -void vpFeatureEllipse::display(const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color, +void vpFeatureEllipse::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { try { @@ -383,8 +376,7 @@ void vpFeatureEllipse::display(const vpCameraParameters &cam, double mu11 = s[3]; double mu02 = s[4]; - vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02, cam, I, color, - thickness); + vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02, cam, I, color, thickness); } catch (...) { vpERROR_TRACE("Error caught"); @@ -401,8 +393,7 @@ void vpFeatureEllipse::display(const vpCameraParameters &cam, \param color : Color used to display the feature. \param thickness : Thickness of the feature representation. */ -void vpFeatureEllipse::display(const vpCameraParameters &cam, - const vpImage &I, const vpColor &color, +void vpFeatureEllipse::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { try { @@ -413,8 +404,7 @@ void vpFeatureEllipse::display(const vpCameraParameters &cam, double mu11 = s[3]; double mu02 = s[4]; - vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02, cam, I, color, - thickness); + vpFeatureDisplay::displayEllipse(x, y, mu20, mu11, mu02, cam, I, color, thickness); } catch (...) { vpERROR_TRACE("Error caught"); diff --git a/modules/visual_features/src/visual-feature/vpFeatureLine.cpp b/modules/visual_features/src/visual-feature/vpFeatureLine.cpp index 02d58cd5c5..f3feae746f 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLine.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLine.cpp @@ -134,8 +134,7 @@ void vpFeatureLine::setRhoTheta(const double rho, const double theta) \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_) +void vpFeatureLine::setABCD(const double A_, const double B_, const double C_, const double D_) { this->A = A_; this->B = B_; @@ -244,8 +243,7 @@ vpMatrix vpFeatureLine::interaction(const unsigned int select) if (fabs(D) < 1e-6) { vpERROR_TRACE("Incorrect plane coordinates D is null, D = %f", D); - throw(vpFeatureException(vpFeatureException::badInitializationError, - "Incorrect plane coordinates D")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Incorrect plane coordinates D")); } double lambda_theta = (A * si - B * co) / D; @@ -317,8 +315,7 @@ vpMatrix vpFeatureLine::interaction(const unsigned int select) s.error(s_star, vpFeatureLine::selectTheta()); \endcode */ -vpColVector vpFeatureLine::error(const vpBasicFeature &s_star, - const unsigned int select) +vpColVector vpFeatureLine::error(const vpBasicFeature &s_star, const unsigned int select) { vpColVector e(0); @@ -372,8 +369,7 @@ vpColVector vpFeatureLine::error(const vpBasicFeature &s_star, void vpFeatureLine::print(const unsigned int select) const { - std::cout << "Line:\t " << A << "X+" << B << "Y+" << C << "Z +" << D - << "=0" << std::endl; + std::cout << "Line:\t " << A << "X+" << B << "Y+" << C << "Z +" << D << "=0" << std::endl; ; if (vpFeatureLine::selectRho() & select) std::cout << " \trho=" << s[0]; @@ -432,9 +428,8 @@ void vpFeatureLine::buildFrom(const double rho, const double theta) \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_) +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; @@ -472,9 +467,7 @@ vpFeatureLine *vpFeatureLine::duplicate() const \param thickness : Thickness of the feature representation. */ -void vpFeatureLine::display(const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color, +void vpFeatureLine::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { try { @@ -500,8 +493,7 @@ void vpFeatureLine::display(const vpCameraParameters &cam, \param thickness : Thickness of the feature representation. */ -void vpFeatureLine::display(const vpCameraParameters &cam, - const vpImage &I, const vpColor &color, +void vpFeatureLine::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { try { diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp index 45b8acd1e2..de8e68695a 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp @@ -79,9 +79,7 @@ void vpFeatureLuminance::init(unsigned int _nbr, unsigned int _nbc, double _Z) nbc = _nbc; if ((nbr < 2 * bord) || (nbc < 2 * bord)) { - throw vpException( - vpException::dimensionError, - "border is too important compared to number of row or column."); + 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 @@ -100,8 +98,7 @@ void vpFeatureLuminance::init(unsigned int _nbr, unsigned int _nbc, double _Z) /*! Default constructor that build a visual feature. */ -vpFeatureLuminance::vpFeatureLuminance() - : Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam() +vpFeatureLuminance::vpFeatureLuminance() : Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam() { nbParameters = 1; dim_s = 0; @@ -114,8 +111,7 @@ vpFeatureLuminance::vpFeatureLuminance() Copy constructor. */ vpFeatureLuminance::vpFeatureLuminance(const vpFeatureLuminance &f) - : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), - firstTimeIn(0), cam() + : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam() { *this = f; } @@ -168,10 +164,7 @@ void vpFeatureLuminance::set_Z(const double Z_) */ double vpFeatureLuminance::get_Z() const { return Z; } -void vpFeatureLuminance::setCameraParameters(vpCameraParameters &_cam) -{ - cam = _cam; -} +void vpFeatureLuminance::setCameraParameters(vpCameraParameters &_cam) { cam = _cam; } /*! @@ -288,8 +281,7 @@ void vpFeatureLuminance::error(const vpBasicFeature &s_star, vpColVector &e) \param select : Not used. */ -vpColVector vpFeatureLuminance::error(const vpBasicFeature &s_star, - const unsigned int /* select */) +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 @@ -321,10 +313,8 @@ void vpFeatureLuminance::print(const unsigned int /* select */) const Not implemented. */ -void vpFeatureLuminance::display(const vpCameraParameters & /* cam */, - const vpImage & /* I */, - const vpColor & /* color */, - unsigned int /* thickness */) const +void vpFeatureLuminance::display(const vpCameraParameters & /* cam */, const vpImage & /* I */, + const vpColor & /* color */, unsigned int /* thickness */) const { static int firsttime = 0; @@ -341,10 +331,8 @@ void vpFeatureLuminance::display(const vpCameraParameters & /* cam */, Not implemented. */ -void vpFeatureLuminance::display(const vpCameraParameters & /* cam */, - const vpImage & /* I */, - const vpColor & /* color */, - unsigned int /* thickness */) const +void vpFeatureLuminance::display(const vpCameraParameters & /* cam */, const vpImage & /* I */, + const vpColor & /* color */, unsigned int /* thickness */) const { static int firsttime = 0; diff --git a/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp b/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp index b158e064f9..a5bed938ac 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp @@ -111,9 +111,7 @@ void vpFeatureMoment::print(unsigned int select) const Not implemented since visual representation of a moment doesn't often make sense. */ -void vpFeatureMoment::display(const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color, +void vpFeatureMoment::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { // visual representation of a moment doesn't often make sense @@ -127,8 +125,7 @@ void vpFeatureMoment::display(const vpCameraParameters &cam, Not implemented since visual representation of a moment doesn't often make sense. */ -void vpFeatureMoment::display(const vpCameraParameters &cam, - const vpImage &I, const vpColor &color, +void vpFeatureMoment::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { (void)cam; @@ -163,8 +160,7 @@ void vpFeatureMoment::update(double A_, double B_, double C_) bool found; this->moment = &(moments.get(momentName(), found)); if (!found) - throw vpException(vpException::notInitialized, - "Moment not found for feature"); + throw vpException(vpException::notInitialized, "Moment not found for feature"); } nbParameters = 1; if (this->moment != NULL) { @@ -223,8 +219,7 @@ vpMatrix vpFeatureMoment::interaction(unsigned int select) */ vpBasicFeature *vpFeatureMoment::duplicate() const { - vpFeatureMoment *feat = new vpMomentGenericFeature( - moments, A, B, C, featureMomentsDataBase, moment); + vpFeatureMoment *feat = new vpMomentGenericFeature(moments, A, B, C, featureMomentsDataBase, moment); feat->interaction_matrices = interaction_matrices; feat->dim_s = dim_s; feat->nbParameters = nbParameters; @@ -249,8 +244,7 @@ vpBasicFeature *vpFeatureMoment::duplicate() const void vpFeatureMoment::linkTo(vpFeatureMomentDatabase &featureMoments) { if (strlen(name()) >= 255) { - throw(vpException(vpException::memoryAllocationError, - "Not enough memory to intialize the moment name")); + throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the moment name")); } std::strcpy(_name, name()); @@ -263,19 +257,16 @@ void vpFeatureMoment::compute_interaction() {} vpFeatureMoment::~vpFeatureMoment() {} -VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpFeatureMoment &featM) +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(featM).interaction( - vpBasicFeature::FEATURE_ALL); + vpMatrix Lcomplete((unsigned int)featM.getDimension(), + 6); // 6 corresponds to 6velocities in standard interaction matrix + Lcomplete = const_cast(featM).interaction(vpBasicFeature::FEATURE_ALL); Lcomplete.matlabPrint(os); return os; } diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentAlpha.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentAlpha.cpp index a0bcf52678..c57bb2ae68 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentAlpha.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentAlpha.cpp @@ -61,32 +61,25 @@ void vpFeatureMomentAlpha::compute_interaction() bool found_FeatureMoment_centered; const vpMomentCentered &momentCentered = - (static_cast( - moments.get("vpMomentCentered", found_moment_centered))); - vpFeatureMomentCentered &featureMomentCentered = - (static_cast(featureMomentsDataBase->get( - "vpFeatureMomentCentered", found_FeatureMoment_centered))); + (static_cast(moments.get("vpMomentCentered", found_moment_centered))); + vpFeatureMomentCentered &featureMomentCentered = (static_cast( + featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered))); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); if (!found_FeatureMoment_centered) - throw vpException(vpException::notInitialized, - "vpFeatureMomentCentered not found"); + 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)); + -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)); + 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 @@ -104,19 +97,15 @@ void vpFeatureMomentAlpha::compute_interaction() bool found_moment_gravity; const vpMomentCentered &momentCentered = - static_cast( - moments.get("vpMomentCentered", found_moment_centered)); + static_cast(moments.get("vpMomentCentered", found_moment_centered)); const vpMomentGravityCenter &momentGravity = - static_cast( - moments.get("vpMomentGravityCenter", found_moment_gravity)); + static_cast(moments.get("vpMomentGravityCenter", found_moment_gravity)); const vpMomentObject &momentObject = moment->getObject(); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); if (!found_moment_gravity) - throw vpException(vpException::notInitialized, - "vpMomentGravityCenter not found"); + throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found"); double mu11 = momentCentered.get(1, 1); double mu20 = momentCentered.get(2, 0); @@ -148,12 +137,10 @@ void vpFeatureMomentAlpha::compute_interaction() 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) + + 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) + + Awy = (beta * (mu21 * (mu02 - mu20) + mu11 * (mu30 - mu12)) + gamma * Xg * mu11 * (mu20 + mu02) + gamma * Yg * (mu20 * (mu02 - mu20) - 2 * mu11_2)) / d; @@ -177,8 +164,7 @@ void vpFeatureMomentAlpha::compute_interaction() interaction_matrices[0][0][WZ] = -1.; } -vpColVector vpFeatureMomentAlpha::error(const vpBasicFeature &s_star, - const unsigned int /* select */) +vpColVector vpFeatureMomentAlpha::error(const vpBasicFeature &s_star, const unsigned int /* select */) { vpColVector e(0); double err = s[0] - s_star[0]; diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentArea.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentArea.cpp index efc732f80c..f4138e6634 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentArea.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentArea.cpp @@ -78,26 +78,21 @@ void vpFeatureMomentArea::compute_interaction() // Get Xg and Yg bool found_xgyg; const vpMomentGravityCenter &momentGravity = - static_cast( - moments.get("vpMomentGravityCenter", found_xgyg)); + static_cast(moments.get("vpMomentGravityCenter", found_xgyg)); if (!found_xgyg) - throw vpException(vpException::notInitialized, - "vpMomentGravityCenter not found"); + throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found"); bool found_m00; - const vpMomentArea &areamoment = static_cast( - moments.get("vpMomentArea", found_m00)); + const vpMomentArea &areamoment = static_cast(moments.get("vpMomentArea", found_m00)); if (!found_m00) - throw vpException(vpException::notInitialized, - "vpMomentArea not found"); + 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::epsilon()); + assert(std::fabs(a - mobj.get(0, 0)) < a * std::numeric_limits::epsilon()); interaction_matrices[0][0][0] = -a * A; interaction_matrices[0][0][1] = -a * B; diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentAreaNormalized.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentAreaNormalized.cpp index a2e33003e5..1418890e31 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentAreaNormalized.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentAreaNormalized.cpp @@ -65,33 +65,25 @@ void vpFeatureMomentAreaNormalized::compute_interaction() bool found_FeatureMoment_centered; bool found_featuremoment_basic; - vpFeatureMomentBasic &featureMomentBasic = - (static_cast(featureMomentsDataBase->get( - "vpFeatureMomentBasic", found_featuremoment_basic))); + vpFeatureMomentBasic &featureMomentBasic = (static_cast( + featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic))); const vpMomentCentered &momentCentered = - static_cast( - moments.get("vpMomentCentered", found_moment_centered)); + static_cast(moments.get("vpMomentCentered", found_moment_centered)); const vpMomentObject &momentObject = moment->getObject(); - const vpMomentAreaNormalized &momentSurfaceNormalized = - static_cast(moments.get( - "vpMomentAreaNormalized", found_moment_surface_normalized)); - vpFeatureMomentCentered &featureMomentCentered = - (static_cast(featureMomentsDataBase->get( - "vpFeatureMomentCentered", found_FeatureMoment_centered))); + const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast( + moments.get("vpMomentAreaNormalized", found_moment_surface_normalized)); + vpFeatureMomentCentered &featureMomentCentered = (static_cast( + featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered))); if (!found_FeatureMoment_centered) - throw vpException(vpException::notInitialized, - "vpFeatureMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found"); if (!found_moment_surface_normalized) - throw vpException(vpException::notInitialized, - "vpMomentAreaNormalized not found"); + throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found"); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); if (!found_featuremoment_basic) - throw vpException(vpException::notInitialized, - "vpFeatureMomentBasic not found"); + throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found"); interaction_matrices.resize(1); interaction_matrices[0].resize(1, 6); double normalized_multiplier; @@ -99,16 +91,14 @@ void vpFeatureMomentAreaNormalized::compute_interaction() 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); + 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); + (-momentSurfaceNormalized.getDesiredDepth() / (2 * a)) * sqrt(momentSurfaceNormalized.getDesiredSurface() / a); interaction_matrices[0] = normalized_multiplier * La; } @@ -139,25 +129,19 @@ void vpFeatureMomentAreaNormalized::compute_interaction() bool found_moment_gravity; const vpMomentCentered &momentCentered = - static_cast( - moments.get("vpMomentCentered", found_moment_centered)); + static_cast(moments.get("vpMomentCentered", found_moment_centered)); const vpMomentGravityCenter &momentGravity = - static_cast( - moments.get("vpMomentGravityCenter", found_moment_gravity)); + static_cast(moments.get("vpMomentGravityCenter", found_moment_gravity)); const vpMomentObject &momentObject = moment->getObject(); - const vpMomentAreaNormalized &momentSurfaceNormalized = - static_cast(moments.get( - "vpMomentAreaNormalized", found_moment_surface_normalized)); + const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast( + moments.get("vpMomentAreaNormalized", found_moment_surface_normalized)); if (!found_moment_surface_normalized) - throw vpException(vpException::notInitialized, - "vpMomentAreaNormalized not found"); + throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found"); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); if (!found_moment_gravity) - throw vpException(vpException::notInitialized, - "vpMomentGravityCenter not found"); + 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); @@ -189,10 +173,8 @@ void vpFeatureMomentAreaNormalized::compute_interaction() 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; + 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; diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentBasic.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentBasic.cpp index db84c244b8..e2285d6d58 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentBasic.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentBasic.cpp @@ -50,9 +50,8 @@ 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) +vpFeatureMomentBasic::vpFeatureMomentBasic(vpMomentDatabase &data_base, double A_, double B_, double C_, + vpFeatureMomentDatabase *featureMoments) : vpFeatureMoment(data_base, A_, B_, C_, featureMoments), order(0) { } @@ -67,8 +66,7 @@ void vpFeatureMomentBasic::compute_interaction() const vpMomentObject &m = moment->getObject(); order = m.getOrder() + 1; interaction_matrices.resize(order * order); - for (std::vector::iterator i = interaction_matrices.begin(); - i != interaction_matrices.end(); ++i) + for (std::vector::iterator i = interaction_matrices.begin(); i != interaction_matrices.end(); ++i) i->resize(1, 6); if (m.getType() == vpMomentObject::DISCRETE) { delta = 0; @@ -87,8 +85,7 @@ void vpFeatureMomentBasic::compute_interaction() 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); + 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); @@ -102,15 +99,11 @@ void vpFeatureMomentBasic::compute_interaction() 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_); + -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_); + (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][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_); } @@ -122,17 +115,13 @@ void vpFeatureMomentBasic::compute_interaction() 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); + -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); + (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][WY] = -(i + 3 * delta) * m.get(ip1_, 0) - i * m.get(im1_, 0); interaction_matrices[i_][0][WZ] = i * m.get(im1_, 1); } @@ -147,24 +136,16 @@ void vpFeatureMomentBasic::compute_interaction() 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_); + -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_); + -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_)) - + (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_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_); } } } @@ -175,8 +156,7 @@ Interaction matrix corresponding to \f$ m_{ij} \f$ moment. \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 +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 " diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentCInvariant.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentCInvariant.cpp index b4f38208a6..fe56b1c6e6 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentCInvariant.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentCInvariant.cpp @@ -67,393 +67,267 @@ void vpFeatureMomentCInvariant::compute_interaction() const vpMomentObject &momentObject = moment->getObject(); const vpMomentCentered &momentCentered = - (static_cast( - moments.get("vpMomentCentered", found_moment_centered))); + (static_cast(moments.get("vpMomentCentered", found_moment_centered))); const vpMomentCInvariant &momentCInvariant = - (static_cast( - moments.get("vpMomentCInvariant", found_moment_cinvariant))); - vpFeatureMomentCentered &featureMomentCentered = - (static_cast(featureMomentsDataBase->get( - "vpFeatureMomentCentered", found_FeatureMoment_centered))); + (static_cast(moments.get("vpMomentCInvariant", found_moment_cinvariant))); + vpFeatureMomentCentered &featureMomentCentered = (static_cast( + featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered))); - vpFeatureMomentBasic &featureMomentBasic = - (static_cast(featureMomentsDataBase->get( - "vpFeatureMomentBasic", found_featuremoment_basic))); + vpFeatureMomentBasic &featureMomentBasic = (static_cast( + featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic))); if (!found_featuremoment_basic) - throw vpException(vpException::notInitialized, - "vpFeatureMomentBasic not found"); + throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found"); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); if (!found_moment_cinvariant) - throw vpException(vpException::notInitialized, - "vpMomentCInvariant not found"); + throw vpException(vpException::notInitialized, "vpMomentCInvariant not found"); if (!found_FeatureMoment_centered) - throw vpException(vpException::notInitialized, - "vpFeatureMomentCentered not found"); + 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[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[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)) + + (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)); + (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)) + + (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.interaction(2, 1) + - 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(2, 1) * - featureMomentCentered.interaction(2, 1) + - 6 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) - - 12 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * - featureMomentCentered.interaction(1, 2) + - 6 * momentCentered.get(1, 2) * pow(momentCentered.get(2, 1), 2) * - featureMomentCentered.interaction(1, 2) - - 2 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * - featureMomentCentered.interaction(0, 3) + - 6 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(0, 3) - - 4 * pow(momentCentered.get(2, 1), 3) * - featureMomentCentered.interaction(0, 3); - - 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 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) - - 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) + - 12 * pow(momentCentered.get(2, 1), 3) * - featureMomentCentered.interaction(2, 1) + - 6 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * - featureMomentCentered.interaction(2, 1) + - 6 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(2, 1) * - featureMomentCentered.interaction(2, 1) - - 6 * momentCentered.get(0, 3) * pow(momentCentered.get(1, 2), 2) * - featureMomentCentered.interaction(2, 1) + - 6 * momentCentered.get(1, 2) * pow(momentCentered.get(3, 0), 2) * - featureMomentCentered.interaction(1, 2) - - 6 * pow(momentCentered.get(2, 1), 2) * momentCentered.get(3, 0) * - featureMomentCentered.interaction(1, 2) - - 6 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) + - 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * - featureMomentCentered.interaction(1, 2) - - 12 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * - momentCentered.get(2, 1) * featureMomentCentered.interaction(1, 2) + - 12 * pow(momentCentered.get(1, 2), 3) * - featureMomentCentered.interaction(1, 2) + - 4 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * - featureMomentCentered.interaction(0, 3) - - 6 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(0, 3) + - 2 * pow(momentCentered.get(2, 1), 3) * - featureMomentCentered.interaction(0, 3) + - 6 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * - featureMomentCentered.interaction(0, 3) - - 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(2, 1) * - featureMomentCentered.interaction(0, 3); - - 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 * momentCentered.get(0, 3) * pow(momentCentered.get(1, 2), 2) * - featureMomentCentered.interaction(3, 0) + - pow(momentCentered.get(0, 3), 3) * - featureMomentCentered.interaction(3, 0) + - 3 * momentCentered.get(1, 2) * pow(momentCentered.get(3, 0), 2) * - featureMomentCentered.interaction(2, 1) - - 6 * pow(momentCentered.get(2, 1), 2) * momentCentered.get(3, 0) * - featureMomentCentered.interaction(2, 1) - - 6 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) + - 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * - featureMomentCentered.interaction(2, 1) - - 9 * momentCentered.get(1, 2) * pow(momentCentered.get(2, 1), 2) * - featureMomentCentered.interaction(2, 1) - - 12 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * - momentCentered.get(2, 1) * featureMomentCentered.interaction(2, 1) + - 3 * pow(momentCentered.get(1, 2), 3) * - featureMomentCentered.interaction(2, 1) - - 3 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(1, 2) * - featureMomentCentered.interaction(2, 1) + - 3 * momentCentered.get(2, 1) * pow(momentCentered.get(3, 0), 2) * - featureMomentCentered.interaction(1, 2) + - 12 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) + - 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) - - 3 * pow(momentCentered.get(2, 1), 3) * - featureMomentCentered.interaction(1, 2) - - 6 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * - featureMomentCentered.interaction(1, 2) + - 9 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(2, 1) * - featureMomentCentered.interaction(1, 2) - - 3 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(2, 1) * - featureMomentCentered.interaction(1, 2) + - 6 * momentCentered.get(0, 3) * pow(momentCentered.get(1, 2), 2) * - featureMomentCentered.interaction(1, 2) - - pow(momentCentered.get(3, 0), 3) * - featureMomentCentered.interaction(0, 3) - - 3 * pow(momentCentered.get(2, 1), 2) * momentCentered.get(3, 0) * - featureMomentCentered.interaction(0, 3) + - 3 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * - featureMomentCentered.interaction(0, 3) + - 3 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(3, 0) * - featureMomentCentered.interaction(0, 3) - - 6 * momentCentered.get(1, 2) * pow(momentCentered.get(2, 1), 2) * - featureMomentCentered.interaction(0, 3) - - 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * - momentCentered.get(2, 1) * featureMomentCentered.interaction(0, 3) + - 2 * pow(momentCentered.get(1, 2), 3) * - featureMomentCentered.interaction(0, 3); - - 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) * momentCentered.get(1, 2) - - 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * - featureMomentCentered.interaction(0, 3) * momentCentered.get(1, 2) - - 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * - momentCentered.get(0, 3) * featureMomentCentered.interaction(1, 2) - - 2 * momentCentered.get(3, 0) * momentCentered.get(1, 2) * - momentCentered.get(0, 3) * featureMomentCentered.interaction(0, 3) + - 6 * momentCentered.get(2, 1) * momentCentered.get(1, 2) * - momentCentered.get(0, 3) * featureMomentCentered.interaction(1, 2) - - pow((double)momentCentered.get(3, 0), (double)3) * - featureMomentCentered.interaction(1, 2) + - 3 * featureMomentCentered.interaction(3, 0) * - pow((double)momentCentered.get(1, 2), (double)3) + - 6 * pow((double)momentCentered.get(2, 1), (double)3) * - featureMomentCentered.interaction(0, 3) - - featureMomentCentered.interaction(2, 1) * - pow((double)momentCentered.get(0, 3), (double)3) + - 3 * featureMomentCentered.interaction(2, 1) * - pow((double)momentCentered.get(1, 2), (double)2) * - momentCentered.get(0, 3) + - 18 * pow((double)momentCentered.get(2, 1), (double)2) * - momentCentered.get(0, 3) * featureMomentCentered.interaction(2, 1) - - pow((double)momentCentered.get(3, 0), (double)2) * - featureMomentCentered.interaction(2, 1) * momentCentered.get(0, 3) + - 9 * momentCentered.get(3, 0) * - pow((double)momentCentered.get(1, 2), (double)2) * - featureMomentCentered.interaction(1, 2) - - 4 * pow((double)momentCentered.get(3, 0), (double)2) * - momentCentered.get(1, 2) * featureMomentCentered.interaction(1, 2) + - 2 * pow((double)momentCentered.get(1, 2), (double)2) * - momentCentered.get(0, 3) * featureMomentCentered.interaction(0, 3) - - 4 * momentCentered.get(3, 0) * - pow((double)momentCentered.get(1, 2), (double)2) * - featureMomentCentered.interaction(3, 0) + - 2 * momentCentered.get(1, 2) * - pow((double)momentCentered.get(0, 3), (double)2) * - featureMomentCentered.interaction(1, 2) - - 4 * momentCentered.get(2, 1) * - pow((double)momentCentered.get(0, 3), (double)2) * - featureMomentCentered.interaction(2, 1) + - 3 * momentCentered.get(3, 0) * - pow((double)momentCentered.get(2, 1), (double)2) * - featureMomentCentered.interaction(1, 2) - - 3 * pow((double)momentCentered.get(3, 0), (double)2) * - momentCentered.get(1, 2) * featureMomentCentered.interaction(3, 0) - - momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) * - pow((double)momentCentered.get(0, 3), (double)2) - - 4 * pow((double)momentCentered.get(2, 1), (double)2) * - momentCentered.get(0, 3) * featureMomentCentered.interaction(0, 3) - - 3 * momentCentered.get(2, 1) * - pow((double)momentCentered.get(0, 3), (double)2) * - featureMomentCentered.interaction(0, 3) + - 2 * momentCentered.get(3, 0) * - pow((double)momentCentered.get(2, 1), (double)2) * - featureMomentCentered.interaction(3, 0) + - 2 * pow((double)momentCentered.get(3, 0), (double)2) * - momentCentered.get(2, 1) * featureMomentCentered.interaction(2, 1) + - 3 * featureMomentCentered.interaction(3, 0) * - pow((double)momentCentered.get(2, 1), (double)2) * - momentCentered.get(1, 2) - - pow((double)momentCentered.get(3, 0), (double)2) * - momentCentered.get(2, 1) * featureMomentCentered.interaction(0, 3) + - 3 * momentCentered.get(2, 1) * - pow((double)momentCentered.get(1, 2), (double)2) * - featureMomentCentered.interaction(0, 3) - - featureMomentCentered.interaction(3, 0) * momentCentered.get(1, 2) * - pow((double)momentCentered.get(0, 3), (double)2); - - 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 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * - momentCentered.get(2, 1) * featureMomentCentered.interaction(3, 0) + - 6 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(1, 2) * - featureMomentCentered.interaction(3, 0) + - 6 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * - featureMomentCentered.interaction(2, 1) + - 18 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) + - 18 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(2, 1) * - featureMomentCentered.interaction(2, 1) + - 6 * pow(momentCentered.get(0, 3), 3) * - featureMomentCentered.interaction(2, 1) + - 6 * pow(momentCentered.get(3, 0), 3) * - featureMomentCentered.interaction(1, 2) + - 18 * momentCentered.get(1, 2) * pow(momentCentered.get(3, 0), 2) * - featureMomentCentered.interaction(1, 2) + - 18 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) + - 6 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(3, 0) * - featureMomentCentered.interaction(1, 2) + - 6 * momentCentered.get(2, 1) * pow(momentCentered.get(3, 0), 2) * - featureMomentCentered.interaction(0, 3) + - 4 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * - featureMomentCentered.interaction(0, 3) + - 18 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(0, 3) + - 12 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * - momentCentered.get(3, 0) * featureMomentCentered.interaction(0, 3) + - 18 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * - featureMomentCentered.interaction(0, 3) + - 18 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(2, 1) * - featureMomentCentered.interaction(0, 3) + - 4 * pow(momentCentered.get(0, 3), 3) * - featureMomentCentered.interaction(0, 3); - - 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) * featureMomentCentered.interaction(1, 3) - - 3 * featureMomentCentered.interaction(2, 2) * momentCentered.get(0, 4) - - 3 * momentCentered.get(2, 2) * featureMomentCentered.interaction(0, 4) + - 6 * momentCentered.get(1, 3) * featureMomentCentered.interaction(1, 3); - - 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) * - featureMomentCentered.interaction(1, 3) + - 12 * featureMomentCentered.interaction(2, 2) * - momentCentered.get(0, 4) + - 12 * momentCentered.get(2, 2) * - featureMomentCentered.interaction(0, 4) + - 6 * momentCentered.get(0, 4) * featureMomentCentered.interaction(0, 4); - - 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(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.interaction(2, 1) + + 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(2, 1) + + 6 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(1, 2) - + 12 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) + + 6 * momentCentered.get(1, 2) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(1, 2) - + 2 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(0, 3) + + 6 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(0, 3) - + 4 * pow(momentCentered.get(2, 1), 3) * featureMomentCentered.interaction(0, 3); + + 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 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(2, 1) - + 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(2, 1) + + 12 * pow(momentCentered.get(2, 1), 3) * featureMomentCentered.interaction(2, 1) + + 6 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(2, 1) + + 6 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(2, 1) - + 6 * momentCentered.get(0, 3) * pow(momentCentered.get(1, 2), 2) * featureMomentCentered.interaction(2, 1) + + 6 * momentCentered.get(1, 2) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(1, 2) - + 6 * pow(momentCentered.get(2, 1), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) - + 6 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(1, 2) + + 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) - + 12 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(2, 1) * + featureMomentCentered.interaction(1, 2) + + 12 * pow(momentCentered.get(1, 2), 3) * featureMomentCentered.interaction(1, 2) + + 4 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(0, 3) - + 6 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(0, 3) + + 2 * pow(momentCentered.get(2, 1), 3) * featureMomentCentered.interaction(0, 3) + + 6 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(0, 3) - + 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(0, 3); + + 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 * momentCentered.get(0, 3) * pow(momentCentered.get(1, 2), 2) * featureMomentCentered.interaction(3, 0) + + pow(momentCentered.get(0, 3), 3) * featureMomentCentered.interaction(3, 0) + + 3 * momentCentered.get(1, 2) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(2, 1) - + 6 * pow(momentCentered.get(2, 1), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) - + 6 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(2, 1) + + 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) - + 9 * momentCentered.get(1, 2) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(2, 1) - + 12 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(2, 1) * + featureMomentCentered.interaction(2, 1) + + 3 * pow(momentCentered.get(1, 2), 3) * featureMomentCentered.interaction(2, 1) - + 3 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(1, 2) * featureMomentCentered.interaction(2, 1) + + 3 * momentCentered.get(2, 1) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(1, 2) + + 12 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(1, 2) + + 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(1, 2) - + 3 * pow(momentCentered.get(2, 1), 3) * featureMomentCentered.interaction(1, 2) - + 6 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(1, 2) + + 9 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(1, 2) - + 3 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(1, 2) + + 6 * momentCentered.get(0, 3) * pow(momentCentered.get(1, 2), 2) * featureMomentCentered.interaction(1, 2) - + pow(momentCentered.get(3, 0), 3) * featureMomentCentered.interaction(0, 3) - + 3 * pow(momentCentered.get(2, 1), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(0, 3) + + 3 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(0, 3) + + 3 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(0, 3) - + 6 * momentCentered.get(1, 2) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(0, 3) - + 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(2, 1) * + featureMomentCentered.interaction(0, 3) + + 2 * pow(momentCentered.get(1, 2), 3) * featureMomentCentered.interaction(0, 3); + + 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) * + momentCentered.get(1, 2) - + 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * featureMomentCentered.interaction(0, 3) * + momentCentered.get(1, 2) - + 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) * + featureMomentCentered.interaction(1, 2) - + 2 * momentCentered.get(3, 0) * momentCentered.get(1, 2) * momentCentered.get(0, 3) * + featureMomentCentered.interaction(0, 3) + + 6 * momentCentered.get(2, 1) * momentCentered.get(1, 2) * momentCentered.get(0, 3) * + featureMomentCentered.interaction(1, 2) - + pow((double)momentCentered.get(3, 0), (double)3) * featureMomentCentered.interaction(1, 2) + + 3 * featureMomentCentered.interaction(3, 0) * pow((double)momentCentered.get(1, 2), (double)3) + + 6 * pow((double)momentCentered.get(2, 1), (double)3) * featureMomentCentered.interaction(0, 3) - + featureMomentCentered.interaction(2, 1) * pow((double)momentCentered.get(0, 3), (double)3) + + 3 * featureMomentCentered.interaction(2, 1) * pow((double)momentCentered.get(1, 2), (double)2) * + momentCentered.get(0, 3) + + 18 * pow((double)momentCentered.get(2, 1), (double)2) * momentCentered.get(0, 3) * + featureMomentCentered.interaction(2, 1) - + pow((double)momentCentered.get(3, 0), (double)2) * featureMomentCentered.interaction(2, 1) * + momentCentered.get(0, 3) + + 9 * momentCentered.get(3, 0) * pow((double)momentCentered.get(1, 2), (double)2) * + featureMomentCentered.interaction(1, 2) - + 4 * pow((double)momentCentered.get(3, 0), (double)2) * momentCentered.get(1, 2) * + featureMomentCentered.interaction(1, 2) + + 2 * pow((double)momentCentered.get(1, 2), (double)2) * momentCentered.get(0, 3) * + featureMomentCentered.interaction(0, 3) - + 4 * momentCentered.get(3, 0) * pow((double)momentCentered.get(1, 2), (double)2) * + featureMomentCentered.interaction(3, 0) + + 2 * momentCentered.get(1, 2) * pow((double)momentCentered.get(0, 3), (double)2) * + featureMomentCentered.interaction(1, 2) - + 4 * momentCentered.get(2, 1) * pow((double)momentCentered.get(0, 3), (double)2) * + featureMomentCentered.interaction(2, 1) + + 3 * momentCentered.get(3, 0) * pow((double)momentCentered.get(2, 1), (double)2) * + featureMomentCentered.interaction(1, 2) - + 3 * pow((double)momentCentered.get(3, 0), (double)2) * momentCentered.get(1, 2) * + featureMomentCentered.interaction(3, 0) - + momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) * + pow((double)momentCentered.get(0, 3), (double)2) - + 4 * pow((double)momentCentered.get(2, 1), (double)2) * momentCentered.get(0, 3) * + featureMomentCentered.interaction(0, 3) - + 3 * momentCentered.get(2, 1) * pow((double)momentCentered.get(0, 3), (double)2) * + featureMomentCentered.interaction(0, 3) + + 2 * momentCentered.get(3, 0) * pow((double)momentCentered.get(2, 1), (double)2) * + featureMomentCentered.interaction(3, 0) + + 2 * pow((double)momentCentered.get(3, 0), (double)2) * momentCentered.get(2, 1) * + featureMomentCentered.interaction(2, 1) + + 3 * featureMomentCentered.interaction(3, 0) * pow((double)momentCentered.get(2, 1), (double)2) * + momentCentered.get(1, 2) - + pow((double)momentCentered.get(3, 0), (double)2) * momentCentered.get(2, 1) * + featureMomentCentered.interaction(0, 3) + + 3 * momentCentered.get(2, 1) * pow((double)momentCentered.get(1, 2), (double)2) * + featureMomentCentered.interaction(0, 3) - + featureMomentCentered.interaction(3, 0) * momentCentered.get(1, 2) * + pow((double)momentCentered.get(0, 3), (double)2); + + 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 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(2, 1) * + featureMomentCentered.interaction(3, 0) + + 6 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(1, 2) * featureMomentCentered.interaction(3, 0) + + 6 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(2, 1) + + 18 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(2, 1) + + 18 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(2, 1) + + 6 * pow(momentCentered.get(0, 3), 3) * featureMomentCentered.interaction(2, 1) + + 6 * pow(momentCentered.get(3, 0), 3) * featureMomentCentered.interaction(1, 2) + + 18 * momentCentered.get(1, 2) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(1, 2) + + 18 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(1, 2) + + 6 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) + + 6 * momentCentered.get(2, 1) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(0, 3) + + 4 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(0, 3) + + 18 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(0, 3) + + 12 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(3, 0) * + featureMomentCentered.interaction(0, 3) + + 18 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(0, 3) + + 18 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(0, 3) + + 4 * pow(momentCentered.get(0, 3), 3) * featureMomentCentered.interaction(0, 3); + + 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) * featureMomentCentered.interaction(1, 3) - + 3 * featureMomentCentered.interaction(2, 2) * momentCentered.get(0, 4) - + 3 * momentCentered.get(2, 2) * featureMomentCentered.interaction(0, 4) + + 6 * momentCentered.get(1, 3) * featureMomentCentered.interaction(1, 3); + + 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) * featureMomentCentered.interaction(1, 3) + + 12 * featureMomentCentered.interaction(2, 2) * momentCentered.get(0, 4) + + 12 * momentCentered.get(2, 2) * featureMomentCentered.interaction(0, 4) + + 6 * momentCentered.get(0, 4) * featureMomentCentered.interaction(0, 4); + + 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) + + 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) - + 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) - + 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) + + 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) + + 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); @@ -464,113 +338,76 @@ void vpFeatureMomentCInvariant::compute_interaction() 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)) + + 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); + (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)); + 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; + 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 @@ -609,32 +446,24 @@ void vpFeatureMomentCInvariant::compute_interaction() const vpMomentObject &momentObject = moment->getObject(); const vpMomentCentered &momentCentered = - (static_cast( - moments.get("vpMomentCentered", found_moment_centered))); + (static_cast(moments.get("vpMomentCentered", found_moment_centered))); const vpMomentCInvariant &momentCInvariant = - (static_cast( - moments.get("vpMomentCInvariant", found_moment_cinvariant))); + (static_cast(moments.get("vpMomentCInvariant", found_moment_cinvariant))); - vpFeatureMomentCentered &featureMomentCentered = - (static_cast(featureMomentsDataBase->get( - "vpFeatureMomentCentered", found_FeatureMoment_centered))); + vpFeatureMomentCentered &featureMomentCentered = (static_cast( + featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered))); - vpFeatureMomentBasic &featureMomentBasic = - (static_cast(featureMomentsDataBase->get( - "vpFeatureMomentBasic", found_featuremoment_basic))); + vpFeatureMomentBasic &featureMomentBasic = (static_cast( + featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic))); if (!found_featuremoment_basic) - throw vpException(vpException::notInitialized, - "vpFeatureMomentBasic not found"); + throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found"); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); if (!found_moment_cinvariant) - throw vpException(vpException::notInitialized, - "vpMomentCInvariant not found"); + throw vpException(vpException::notInitialized, "vpMomentCInvariant not found"); if (!found_FeatureMoment_centered) - throw vpException(vpException::notInitialized, - "vpFeatureMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found"); vpMatrix zeros(1, 6); for (int i = 0; i < 6; i++) @@ -691,108 +520,78 @@ void vpFeatureMomentCInvariant::compute_interaction() 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[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[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[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) * + (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 * mu30 * mu12 * mu03 - 12 * mu21_2 * mu03 + 6 * mu21 * mu12_2) * + (6 * mu21 * mu03_2 + 6 * mu21_2 * mu03 - 6 * mu30 * mu12 * mu03 + 12 * mu21_3 - 12 * mu30 * mu21 * mu12 - + 6 * mu12_2 * mu03) * Lmu21 + - (-2 * mu30 * mu03_2 - 4 * mu12_3 + 6 * mu21 * mu12 * mu03) * Lmu30; + (6 * mu30 * mu12_2 + 2 * mu12_3 + 4 * mu30 * mu03_2 - 6 * mu21_2 * mu12 - 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) * + 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 + - (-6 * mu30 * mu21_2 - 6 * mu30 * mu21 * mu03 + 12 * mu12_3 + - 6 * mu30_2 * mu12 - 12 * mu21 * mu12 * mu03 + 6 * mu30 * mu12_2) * + (-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 + - (6 * mu21 * mu03_2 + 6 * mu21_2 * mu03 - 6 * mu30 * mu12 * mu03 + - 12 * mu21_3 - 12 * mu30 * mu21 * mu12 - 6 * mu12_2 * mu03) * + (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 * mu30 * mu12_2 + 2 * mu12_3 + 4 * mu30 * mu03_2 - - 6 * mu21_2 * mu12 - 6 * mu21 * mu12 * mu03) * + (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[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) * + 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) * + (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) * + (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 + + (-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; + 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); @@ -810,16 +609,14 @@ void vpFeatureMomentCInvariant::compute_interaction() 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 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)); + La = (featureMomentCentered.interaction(2, 0) + featureMomentCentered.interaction(0, 2)); } else { a = momentObject.get(0, 0); La = featureMomentBasic.interaction(0, 0); @@ -832,74 +629,50 @@ void vpFeatureMomentCInvariant::compute_interaction() 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; + 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); @@ -926,8 +699,7 @@ void vpFeatureMomentCInvariant::printLsofInvariants(std::ostream &os) const \relates vpFeatureMomentCInvariant Print all the interaction matrices of visual features */ -std::ostream &operator<<(std::ostream &os, - const vpFeatureMomentCInvariant &featcinv) +std::ostream &operator<<(std::ostream &os, const vpFeatureMomentCInvariant &featcinv) { // Print L for c1 .. c10 for (unsigned int i = 0; i < 10; ++i) { diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp index 30efd192c2..5cb899e9d1 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp @@ -58,9 +58,8 @@ 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) +vpFeatureMomentCentered::vpFeatureMomentCentered(vpMomentDatabase &moments_, double A_, double B_, double C_, + vpFeatureMomentDatabase *featureMoments) : vpFeatureMoment(moments_, A_, B_, C_, featureMoments), order(0) { } @@ -71,8 +70,7 @@ Interaction matrix corresponding to \f$ \mu_{ij} \f$ moment \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 +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 " @@ -86,11 +84,10 @@ vpMatrix vpFeatureMomentCentered::interaction(unsigned int select_one, * 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 +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 @@ -123,8 +120,7 @@ vpMatrix vpFeatureMomentCentered::compute_Lmu_pq( 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; + Lterm3 += pcombkqcombl * pow(xg, pmk) * pow(yg, qml) * L_mkl * minus1pow; } } @@ -151,51 +147,42 @@ void vpFeatureMomentCentered::compute_interaction() const vpMomentObject &momentObject = moment->getObject(); order = momentObject.getOrder() + 1; interaction_matrices.resize(order * order); - for (std::vector::iterator i = interaction_matrices.begin(); - i != interaction_matrices.end(); ++i) + for (std::vector::iterator i = interaction_matrices.begin(); i != interaction_matrices.end(); ++i) i->resize(1, 6); bool found_moment_gravity; const vpMomentGravityCenter &momentGravity = - static_cast( - moments.get("vpMomentGravityCenter", found_moment_gravity)); + static_cast(moments.get("vpMomentGravityCenter", found_moment_gravity)); if (!found_moment_gravity) - throw vpException(vpException::notInitialized, - "vpMomentGravityCenter not found"); + 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( - featureMomentsDataBase->get("vpFeatureMomentGravityCenter", - found_feature_gravity_center))); + vpFeatureMomentGravityCenter &featureMomentGravityCenter = (static_cast( + featureMomentsDataBase->get("vpFeatureMomentGravityCenter", found_feature_gravity_center))); if (!found_feature_gravity_center) - throw vpException(vpException::notInitialized, - "vpFeatureMomentGravityCenter not found"); + 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( - moments.get("vpMomentBasic", found_moment_basic)); + const vpMomentBasic &momentbasic = + static_cast(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(featureMomentsDataBase->get( - "vpFeatureMomentBasic", found_featuremoment_basic))); + vpFeatureMomentBasic &featureMomentBasic = (static_cast( + featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic))); if (!found_featuremoment_basic) - throw vpException(vpException::notInitialized, - "vpFeatureMomentBasic not found"); + 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); + compute_Lmu_pq(i, j, xg, yg, Lxg, Lyg, momentbasic, featureMomentBasic); } } #else // #ifdef VISP_MOMENTS_COMBINE_MATRICES @@ -203,26 +190,21 @@ void vpFeatureMomentCentered::compute_interaction() bool found_moment_gravity; const vpMomentCentered &momentCentered = - (static_cast( - moments.get("vpMomentCentered", found_moment_centered))); + (static_cast(moments.get("vpMomentCentered", found_moment_centered))); const vpMomentGravityCenter &momentGravity = - static_cast( - moments.get("vpMomentGravityCenter", found_moment_gravity)); + static_cast(moments.get("vpMomentGravityCenter", found_moment_gravity)); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); if (!found_moment_gravity) - throw vpException(vpException::notInitialized, - "vpMomentGravityCenter not found"); + 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::iterator i = interaction_matrices.begin(); - i != interaction_matrices.end(); ++i) + for (std::vector::iterator i = interaction_matrices.begin(); i != interaction_matrices.end(); ++i) i->resize(1, 6); if (momentObject.getType() == vpMomentObject::DISCRETE) { delta = 0; @@ -254,9 +236,8 @@ void vpFeatureMomentCentered::compute_interaction() // 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][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++) { @@ -270,19 +251,15 @@ void vpFeatureMomentCentered::compute_interaction() 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][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; + (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; } @@ -298,19 +275,14 @@ void vpFeatureMomentCentered::compute_interaction() 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][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; + (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; + -(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; } @@ -331,25 +303,19 @@ void vpFeatureMomentCentered::compute_interaction() 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; + 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 diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentCommon.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentCommon.cpp index 2b46f8561b..3885fe9013 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentCommon.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentCommon.cpp @@ -48,13 +48,10 @@ 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) +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); diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp index 326ed5204d..d55f9717fd 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp @@ -50,8 +50,7 @@ */ void vpFeatureMomentDatabase::add(vpFeatureMoment &featureMoment, char *name) { - featureMomentsDataBase.insert(std::pair( - (const char *)name, &featureMoment)); + featureMomentsDataBase.insert(std::pair((const char *)name, &featureMoment)); } /*! @@ -64,8 +63,7 @@ void vpFeatureMomentDatabase::add(vpFeatureMoment &featureMoment, char *name) */ vpFeatureMoment &vpFeatureMomentDatabase::get(const char *type, bool &found) { - std::map::const_iterator it = + std::map::const_iterator it = featureMomentsDataBase.find(type); found = (it != featureMomentsDataBase.end()); @@ -81,13 +79,11 @@ vpFeatureMoment &vpFeatureMomentDatabase::get(const char *type, bool &found) */ void vpFeatureMomentDatabase::updateAll(double A, double B, double C) { - std::map::const_iterator itr; + std::map::const_iterator itr; #ifdef VISP_HAVE_OPENMP std::vector values; values.reserve(featureMomentsDataBase.size()); - for (itr = featureMomentsDataBase.begin(); - itr != featureMomentsDataBase.end(); ++itr) { + for (itr = featureMomentsDataBase.begin(); itr != featureMomentsDataBase.end(); ++itr) { values.push_back((*itr).second); } #pragma omp parallel for shared(A, B, C) @@ -96,8 +92,7 @@ void vpFeatureMomentDatabase::updateAll(double A, double B, double C) values[i_]->update(A, B, C); } #else - for (itr = featureMomentsDataBase.begin(); - itr != featureMomentsDataBase.end(); ++itr) { + for (itr = featureMomentsDataBase.begin(); itr != featureMomentsDataBase.end(); ++itr) { (*itr).second->update(A, B, C); } #endif diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenter.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenter.cpp index 7115255a97..44f204ba09 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenter.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenter.cpp @@ -60,26 +60,22 @@ void vpFeatureMomentGravityCenter::compute_interaction() { bool found_featuremoment_basic; - vpFeatureMomentBasic &featureMomentBasic = - (static_cast(featureMomentsDataBase->get( - "vpFeatureMomentBasic", found_featuremoment_basic))); + vpFeatureMomentBasic &featureMomentBasic = (static_cast( + featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic))); const vpMomentObject &momentObject = moment->getObject(); if (!found_featuremoment_basic) - throw vpException(vpException::notInitialized, - "vpFeatureMomentBasic not found"); + 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); + 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); + momentObject.get(0, 1) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0); } #else @@ -108,20 +104,16 @@ void vpFeatureMomentGravityCenter::compute_interaction() bool found_moment_gravity; const vpMomentCentered &momentCentered = - (static_cast( - moments.get("vpMomentCentered", found_moment_centered))); + (static_cast(moments.get("vpMomentCentered", found_moment_centered))); const vpMomentGravityCenter &momentGravity = - static_cast( - moments.get("vpMomentGravityCenter", found_moment_gravity)); + static_cast(moments.get("vpMomentGravityCenter", found_moment_gravity)); const vpMomentObject &momentObject = moment->getObject(); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); if (!found_moment_gravity) - throw vpException(vpException::notInitialized, - "vpMomentGravityCenter not found"); + throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found"); interaction_matrices[0].resize(1, 6); interaction_matrices[1].resize(1, 6); diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp index 07f9c25e49..57a3f5da47 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp @@ -69,45 +69,32 @@ void vpFeatureMomentGravityCenterNormalized::compute_interaction() bool found_featuremoment_gravity; bool found_featuremoment_surfacenormalized; - const vpMomentAreaNormalized &momentSurfaceNormalized = - static_cast(moments.get( - "vpMomentAreaNormalized", found_moment_surface_normalized)); + const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast( + moments.get("vpMomentAreaNormalized", found_moment_surface_normalized)); const vpMomentGravityCenter &momentGravity = - static_cast( - moments.get("vpMomentGravityCenter", found_moment_gravity)); - vpFeatureMomentGravityCenter &featureMomentGravity = - (static_cast( - featureMomentsDataBase->get("vpFeatureMomentGravityCenter", - found_featuremoment_gravity))); - vpFeatureMomentAreaNormalized featureMomentAreaNormalized = - (static_cast( - featureMomentsDataBase->get( - "vpFeatureMomentAreaNormalized", - found_featuremoment_surfacenormalized))); + static_cast(moments.get("vpMomentGravityCenter", found_moment_gravity)); + vpFeatureMomentGravityCenter &featureMomentGravity = (static_cast( + featureMomentsDataBase->get("vpFeatureMomentGravityCenter", found_featuremoment_gravity))); + vpFeatureMomentAreaNormalized featureMomentAreaNormalized = (static_cast( + featureMomentsDataBase->get("vpFeatureMomentAreaNormalized", found_featuremoment_surfacenormalized))); if (!found_moment_surface_normalized) - throw vpException(vpException::notInitialized, - "vpMomentAreaNormalized not found"); + throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found"); if (!found_moment_gravity) - throw vpException(vpException::notInitialized, - "vpMomentGravityCenter not found"); + throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found"); if (!found_featuremoment_gravity) - throw vpException(vpException::notInitialized, - "vpFeatureMomentGravityCenter not found"); + throw vpException(vpException::notInitialized, "vpFeatureMomentGravityCenter not found"); if (!found_featuremoment_surfacenormalized) - throw vpException(vpException::notInitialized, - "vpFeatureMomentAreaNormalized not found"); + 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); + 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 @@ -138,24 +125,18 @@ void vpFeatureMomentGravityCenterNormalized::compute_interaction() bool found_moment_centered; const vpMomentCentered &momentCentered = - static_cast( - moments.get("vpMomentCentered", found_moment_centered)); + static_cast(moments.get("vpMomentCentered", found_moment_centered)); const vpMomentGravityCenter &momentGravity = - static_cast( - moments.get("vpMomentGravityCenter", found_moment_gravity)); - const vpMomentAreaNormalized &momentSurfaceNormalized = - static_cast(moments.get( - "vpMomentAreaNormalized", found_moment_surface_normalized)); + static_cast(moments.get("vpMomentGravityCenter", found_moment_gravity)); + const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast( + moments.get("vpMomentAreaNormalized", found_moment_surface_normalized)); if (!found_moment_surface_normalized) - throw vpException(vpException::notInitialized, - "vpMomentAreaNormalized not found"); + throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found"); if (!found_moment_gravity) - throw vpException(vpException::notInitialized, - "vpMomentGravityCenter not found"); + throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found"); if (!found_moment_centered) - throw vpException(vpException::notInitialized, - "vpMomentCentered not found"); + throw vpException(vpException::notInitialized, "vpMomentCentered not found"); const vpMomentObject &momentObject = moment->getObject(); interaction_matrices[0].resize(1, 6); @@ -192,20 +173,17 @@ void vpFeatureMomentGravityCenterNormalized::compute_interaction() 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 + + 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 + + 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 + + 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 { diff --git a/modules/visual_features/src/visual-feature/vpFeaturePoint.cpp b/modules/visual_features/src/visual-feature/vpFeaturePoint.cpp index fc8ac4fa95..78eb5e9a02 100644 --- a/modules/visual_features/src/visual-feature/vpFeaturePoint.cpp +++ b/modules/visual_features/src/visual-feature/vpFeaturePoint.cpp @@ -160,8 +160,7 @@ double vpFeaturePoint::get_y() const { return s[1]; } \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_) +void vpFeaturePoint::set_xyZ(const double x_, const double y_, const double Z_) { set_x(x_); set_y(y_); @@ -251,16 +250,14 @@ vpMatrix vpFeaturePoint::interaction(const unsigned int select) vpERROR_TRACE("Point is behind the camera "); std::cout << "Z = " << Z_ << std::endl; - throw(vpFeatureException(vpFeatureException::badInitializationError, - "Point is behind the camera ")); + 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")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null")); } if (vpFeaturePoint::selectX() & select) { @@ -312,7 +309,8 @@ vpMatrix vpFeaturePoint::interaction(const unsigned int select) feature. The code below shows how to use this method to manipulate the \f$ x \f$ - subset: \code + subset: + \code // Creation of the current feature s vpFeaturePoint s; s.buildFrom(0, 0, 1); @@ -328,8 +326,7 @@ vpMatrix vpFeaturePoint::interaction(const unsigned int select) s.error(s_star, vpFeaturePoint::selectX()); \endcode */ -vpColVector vpFeaturePoint::error(const vpBasicFeature &s_star, - const unsigned int select) +vpColVector vpFeaturePoint::error(const vpBasicFeature &s_star, const unsigned int select) { vpColVector e(0); @@ -395,8 +392,7 @@ void vpFeaturePoint::print(const unsigned int select) const \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_) +void vpFeaturePoint::buildFrom(const double x_, const double y_, const double Z_) { s[0] = x_; @@ -408,16 +404,14 @@ void vpFeaturePoint::buildFrom(const double x_, const double y_, vpERROR_TRACE("Point is behind the camera "); std::cout << "Z = " << Z_ << std::endl; - throw(vpFeatureException(vpFeatureException::badInitializationError, - "Point is behind the camera ")); + 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")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null")); } for (unsigned int i = 0; i < nbParameters; i++) @@ -434,9 +428,7 @@ void vpFeaturePoint::buildFrom(const double x_, const double y_, \param thickness : Thickness of the feature representation. */ -void vpFeaturePoint::display(const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color, +void vpFeaturePoint::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { try { @@ -462,8 +454,7 @@ void vpFeaturePoint::display(const vpCameraParameters &cam, \param thickness : Thickness of the feature representation. */ -void vpFeaturePoint::display(const vpCameraParameters &cam, - const vpImage &I, const vpColor &color, +void vpFeaturePoint::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { try { diff --git a/modules/visual_features/src/visual-feature/vpFeaturePoint3D.cpp b/modules/visual_features/src/visual-feature/vpFeaturePoint3D.cpp index 605c4046a4..29366db337 100644 --- a/modules/visual_features/src/visual-feature/vpFeaturePoint3D.cpp +++ b/modules/visual_features/src/visual-feature/vpFeaturePoint3D.cpp @@ -209,8 +209,8 @@ double vpFeaturePoint3D::get_Z() const { return s[2]; } subset visual feature: \code - vpMatrix L_XY = s.interaction( vpFeaturePoint3D::selectX() | - vpFeaturePoint3D::selectY() ); \endcode + 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$ @@ -351,11 +351,11 @@ vpMatrix vpFeaturePoint3D::interaction(const unsigned int select) // Compute the error vector e = (s-s*) for the Y, Z feature coordinates vpColVector e = s.error(s_star, vpFeaturePoint3D::selectY() | - vpFeaturePoint3D::selectZ()); \endcode + vpFeaturePoint3D::selectZ()); + \endcode */ -vpColVector vpFeaturePoint3D::error(const vpBasicFeature &s_star, - const unsigned int select) +vpColVector vpFeaturePoint3D::error(const vpBasicFeature &s_star, const unsigned int select) { vpColVector e(0); @@ -415,16 +415,14 @@ void vpFeaturePoint3D::buildFrom(const vpPoint &p) vpERROR_TRACE("Point is behind the camera "); std::cout << "Z = " << Z << std::endl; - throw(vpFeatureException(vpFeatureException::badInitializationError, - "Point is behind the camera ")); + 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")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null")); } for (unsigned int i = 0; i < nbParameters; i++) @@ -447,8 +445,7 @@ void vpFeaturePoint3D::buildFrom(const vpPoint &p) on the camera which is not possible. */ -void vpFeaturePoint3D::buildFrom(const double X, const double Y, - const double Z) +void vpFeaturePoint3D::buildFrom(const double X, const double Y, const double Z) { s[0] = X; @@ -459,16 +456,14 @@ void vpFeaturePoint3D::buildFrom(const double X, const double Y, vpERROR_TRACE("Point is behind the camera "); std::cout << "Z = " << Z << std::endl; - throw(vpFeatureException(vpFeatureException::badInitializationError, - "Point is behind the camera ")); + 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")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null")); } for (unsigned int i = 0; i < nbParameters; i++) @@ -532,10 +527,8 @@ vpFeaturePoint3D *vpFeaturePoint3D::duplicate() const Not implemented. */ -void vpFeaturePoint3D::display(const vpCameraParameters & /*cam*/, - const vpImage & /* I */, - const vpColor & /* color */, - unsigned int /* thickness */) const +void vpFeaturePoint3D::display(const vpCameraParameters & /*cam*/, const vpImage & /* I */, + const vpColor & /* color */, unsigned int /* thickness */) const { static int firsttime = 0; @@ -551,10 +544,8 @@ void vpFeaturePoint3D::display(const vpCameraParameters & /*cam*/, Not implemented. */ -void vpFeaturePoint3D::display(const vpCameraParameters & /*cam*/, - const vpImage & /* I */, - const vpColor & /* color */, - unsigned int /* thickness */) const +void vpFeaturePoint3D::display(const vpCameraParameters & /*cam*/, const vpImage & /* I */, + const vpColor & /* color */, unsigned int /* thickness */) const { static int firsttime = 0; @@ -584,7 +575,8 @@ void vpFeaturePoint3D::display(const vpCameraParameters & /*cam*/, ... // Add the (X,Y) subset coordinates features from a 3D point to the task task.addFeature(p, vpFeaturePoint3D::selectX() | - vpFeaturePoint3D::selectY()); \endcode + vpFeaturePoint3D::selectY()); + \endcode \sa selectY(), selectZ() @@ -610,7 +602,8 @@ unsigned int vpFeaturePoint3D::selectX() { return FEATURE_LINE[0]; } ... // Add the (X,Y) subset coordinates features from a 3D point to the task task.addFeature(p, vpFeaturePoint3D::selectX() | - vpFeaturePoint3D::selectY()); \endcode + vpFeaturePoint3D::selectY()); + \endcode \sa selectX(), selectZ() diff --git a/modules/visual_features/src/visual-feature/vpFeaturePointPolar.cpp b/modules/visual_features/src/visual-feature/vpFeaturePointPolar.cpp index 30cfd55a54..0407984f6e 100644 --- a/modules/visual_features/src/visual-feature/vpFeaturePointPolar.cpp +++ b/modules/visual_features/src/visual-feature/vpFeaturePointPolar.cpp @@ -142,8 +142,7 @@ void vpFeaturePointPolar::set_Z(const double Z_) \sa set_rho(), set_theta(), set_Z() */ -void vpFeaturePointPolar::set_rhoThetaZ(const double rho, const double theta, - const double Z_) +void vpFeaturePointPolar::set_rhoThetaZ(const double rho, const double theta, const double Z_) { set_rho(rho); set_theta(theta); @@ -289,24 +288,21 @@ vpMatrix vpFeaturePointPolar::interaction(const unsigned int select) 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")); + 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 ")); + 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")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null")); } if (vpFeaturePointPolar::selectRho() & select) { @@ -386,8 +382,7 @@ vpMatrix vpFeaturePointPolar::interaction(const unsigned int select) vpColVector e = s.error(s_star, vpFeaturePointPolar::selectRho()); \endcode */ -vpColVector vpFeaturePointPolar::error(const vpBasicFeature &s_star, - const unsigned int select) +vpColVector vpFeaturePointPolar::error(const vpBasicFeature &s_star, const unsigned int select) { vpColVector e(0); @@ -472,8 +467,7 @@ void vpFeaturePointPolar::print(const unsigned int select) const (\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_) +void vpFeaturePointPolar::buildFrom(const double rho, const double theta, const double Z_) { s[0] = rho; @@ -485,16 +479,14 @@ void vpFeaturePointPolar::buildFrom(const double rho, const double theta, vpERROR_TRACE("Point is behind the camera "); std::cout << "Z = " << Z << std::endl; - throw(vpFeatureException(vpFeatureException::badInitializationError, - "Point is behind the camera ")); + 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")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null")); } for (unsigned int i = 0; i < nbParameters; i++) @@ -511,9 +503,7 @@ void vpFeaturePointPolar::buildFrom(const double rho, const double theta, \param thickness : Thickness of the feature representation. */ -void vpFeaturePointPolar::display(const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color, +void vpFeaturePointPolar::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { try { @@ -542,9 +532,7 @@ void vpFeaturePointPolar::display(const vpCameraParameters &cam, \param thickness : Thickness of the feature representation. */ -void vpFeaturePointPolar::display(const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color, +void vpFeaturePointPolar::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { try { @@ -599,7 +587,8 @@ vpFeaturePointPolar *vpFeaturePointPolar::duplicate() const vpServo task; ... // Add only the rho subset coordinate feature from an image point to the - task task.addFeature(p, vpFeaturePointPolar::selectRho()); \endcode + task task.addFeature(p, vpFeaturePointPolar::selectRho()); + \endcode \sa selectTheta() */ diff --git a/modules/visual_features/src/visual-feature/vpFeatureSegment.cpp b/modules/visual_features/src/visual-feature/vpFeatureSegment.cpp index d71b368d16..9e9782fe7b 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureSegment.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureSegment.cpp @@ -84,8 +84,7 @@ void vpFeatureSegment::init() 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) + : xc_(0), yc_(0), l_(0), alpha_(0), Z1_(0), Z2_(0), cos_a_(0), sin_a_(0), normalized_(normalized) { init(); } @@ -162,13 +161,12 @@ int main() // 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(); + // 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()); + 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 ); @@ -182,8 +180,8 @@ p2.get_Z()); previous code to consider as visual feature \f$s = (l, \alpha)\f$. \code - vpMatrix L = s.interaction( vpFeatureSegment::selectL() | -vpFeatureSegment::selectAlpha() ); \endcode + vpMatrix L = s.interaction( vpFeatureSegment::selectL() | vpFeatureSegment::selectAlpha() ); + \endcode In that case, L is a 2 by 6 matrix. */ @@ -362,8 +360,7 @@ vpMatrix vpFeatureSegment::interaction(const unsigned int select) visual feature. */ -vpColVector vpFeatureSegment::error(const vpBasicFeature &s_star, - const unsigned int select) +vpColVector vpFeatureSegment::error(const vpBasicFeature &s_star, const unsigned int select) { vpColVector e(0); @@ -410,8 +407,8 @@ vpColVector vpFeatureSegment::error(const vpBasicFeature &s_star, produces the following output: \code - vpFeatureSegment: (xc = -0.255634; yc = -0.13311; l = 0.105005; alpha - = 92.1305 deg) \endcode + vpFeatureSegment: (xc = -0.255634; yc = -0.13311; l = 0.105005; alpha = 92.1305 deg) + \endcode while \code @@ -483,9 +480,7 @@ vpFeatureSegment *vpFeatureSegment::duplicate() const \param thickness : Thickness of the feature representation. */ -void vpFeatureSegment::display(const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color, +void vpFeatureSegment::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { double l, x, y; @@ -523,8 +518,7 @@ void vpFeatureSegment::display(const vpCameraParameters &cam, \param color : Color to use for the segment. \param thickness : Thickness of the feature representation. */ -void vpFeatureSegment::display(const vpCameraParameters &cam, - const vpImage &I, const vpColor &color, +void vpFeatureSegment::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { double l, x, y; @@ -569,9 +563,8 @@ void vpFeatureSegment::display(const vpCameraParameters &cam, - 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) +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.; diff --git a/modules/visual_features/src/visual-feature/vpFeatureThetaU.cpp b/modules/visual_features/src/visual-feature/vpFeatureThetaU.cpp index 3c8bbe3ba5..2f1c5aec14 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureThetaU.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureThetaU.cpp @@ -98,8 +98,7 @@ vpFeatureThetaU::vpFeatureThetaU() : rotation(vpFeatureThetaU::cdRc) visual feature. */ -vpFeatureThetaU::vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r) - : rotation(r) +vpFeatureThetaU::vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r) : rotation(r) { // vpTRACE("0x%x", this); init(); @@ -127,9 +126,7 @@ vpFeatureThetaU::vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r) \param r [in] : The rotation representation of \f$ \theta u \f$. */ -vpFeatureThetaU::vpFeatureThetaU(vpThetaUVector &tu, - vpFeatureThetaURotationRepresentationType r) - : rotation(r) +vpFeatureThetaU::vpFeatureThetaU(vpThetaUVector &tu, vpFeatureThetaURotationRepresentationType r) : rotation(r) { init(); @@ -153,9 +150,7 @@ vpFeatureThetaU::vpFeatureThetaU(vpThetaUVector &tu, */ -vpFeatureThetaU::vpFeatureThetaU(vpRotationMatrix &R, - vpFeatureThetaURotationRepresentationType r) - : rotation(r) +vpFeatureThetaU::vpFeatureThetaU(vpRotationMatrix &R, vpFeatureThetaURotationRepresentationType r) : rotation(r) { init(); @@ -182,9 +177,7 @@ vpFeatureThetaU::vpFeatureThetaU(vpRotationMatrix &R, */ -vpFeatureThetaU::vpFeatureThetaU(vpHomogeneousMatrix &M, - vpFeatureThetaURotationRepresentationType r) - : rotation(r) +vpFeatureThetaU::vpFeatureThetaU(vpHomogeneousMatrix &M, vpFeatureThetaURotationRepresentationType r) : rotation(r) { init(); vpRotationMatrix R; @@ -273,11 +266,7 @@ void vpFeatureThetaU::buildFrom(const vpHomogeneousMatrix &M) vpFeatureThetaU::cRcd. \sa getFeatureThetaURotationType() */ -void vpFeatureThetaU::setFeatureThetaURotationType( - const vpFeatureThetaURotationRepresentationType r) -{ - rotation = r; -} +void vpFeatureThetaU::setFeatureThetaURotationType(const vpFeatureThetaURotationRepresentationType r) { rotation = r; } /*! @@ -327,8 +316,7 @@ void vpFeatureThetaU::set_TUz(const double tu_z) vpFeatureThetaU::cRcd. \sa setFeatureThetaURotationType() */ -vpFeatureThetaU::vpFeatureThetaURotationRepresentationType -vpFeatureThetaU::getFeatureThetaURotationType() const +vpFeatureThetaU::vpFeatureThetaURotationRepresentationType vpFeatureThetaU::getFeatureThetaURotationType() const { return rotation; } @@ -399,8 +387,8 @@ double vpFeatureThetaU::get_TUz() const { return s[2]; } subset visual feature: \code - vpMatrix L_xy = s.interaction( vpFeatureThetaU::selectTUx() | - vpFeatureThetaU::selectTUy() ); \endcode + 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$ @@ -466,8 +454,7 @@ vpMatrix vpFeatureThetaU::interaction(const unsigned int select) vpMatrix skew_u; skew_u = vpColVector::skew(u); - U2 += (1 - vpMath::sinc(theta) / vpMath::sqr(vpMath::sinc(theta / 2.0))) * - skew_u * skew_u; + U2 += (1 - vpMath::sinc(theta) / vpMath::sqr(vpMath::sinc(theta / 2.0))) * skew_u * skew_u; } if (rotation == cdRc) { @@ -573,18 +560,16 @@ vpMatrix vpFeatureThetaU::interaction(const unsigned int select) 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 e = s.error(s_star, vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz()); + \endcode */ -vpColVector vpFeatureThetaU::error(const vpBasicFeature &s_star, - const unsigned int select) +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 !")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "s* should be zero !")); } vpColVector e(0); @@ -677,10 +662,8 @@ vpFeatureThetaU *vpFeatureThetaU::duplicate() const Not implemented. */ -void vpFeatureThetaU::display(const vpCameraParameters & /* cam */, - const vpImage & /* I */, - const vpColor & /* color */, - unsigned int /* thickness */) const +void vpFeatureThetaU::display(const vpCameraParameters & /* cam */, const vpImage & /* I */, + const vpColor & /* color */, unsigned int /* thickness */) const { static int firsttime = 0; @@ -696,10 +679,8 @@ void vpFeatureThetaU::display(const vpCameraParameters & /* cam */, Not implemented. */ -void vpFeatureThetaU::display(const vpCameraParameters & /* cam */, - const vpImage & /* I */, - const vpColor & /* color */, - unsigned int /* thickness */) const +void vpFeatureThetaU::display(const vpCameraParameters & /* cam */, const vpImage & /* I */, + const vpColor & /* color */, unsigned int /* thickness */) const { static int firsttime = 0; @@ -730,8 +711,8 @@ void vpFeatureThetaU::display(const vpCameraParameters & /* cam */, ... // Add the (ThetaU_x, ThetaU_y) subset features from the 3D ThetaU // rotation to the task - task.addFeature(tu, vpFeatureThetaU::selectTUx() | - vpFeatureThetaU::selectTUy()); \endcode + task.addFeature(tu, vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy()); + \endcode \sa selectTUy(), selectTUz() */ @@ -755,8 +736,8 @@ unsigned int vpFeatureThetaU::selectTUx() { return FEATURE_LINE[0]; } ... // Add the (ThetaU_x, ThetaU_y) subset features from the 3D ThetaU // rotation to the task - task.addFeature(tu, vpFeatureThetaU::selectTUx() | - vpFeatureThetaU::selectTUy()); \endcode + task.addFeature(tu, vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy()); + \endcode \sa selectTUx(), selectTUz() */ diff --git a/modules/visual_features/src/visual-feature/vpFeatureTranslation.cpp b/modules/visual_features/src/visual-feature/vpFeatureTranslation.cpp index 0a10c816cc..07a2c2ddd6 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureTranslation.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureTranslation.cpp @@ -85,11 +85,7 @@ void vpFeatureTranslation::init() by default. Use the function setFeatureTranslationType() to set the desired type of feature. */ -vpFeatureTranslation::vpFeatureTranslation() - : f2Mf1(), translation(vpFeatureTranslation::cdMc) -{ - init(); -} +vpFeatureTranslation::vpFeatureTranslation() : f2Mf1(), translation(vpFeatureTranslation::cdMc) { init(); } /*! Default constructor that builds a visual feature and initialize it to zero @@ -98,9 +94,7 @@ vpFeatureTranslation::vpFeatureTranslation() \param r : Type of considered 3D translation feature. */ -vpFeatureTranslation::vpFeatureTranslation( - vpFeatureTranslationRepresentationType r) - : f2Mf1(), translation(r) +vpFeatureTranslation::vpFeatureTranslation(vpFeatureTranslationRepresentationType r) : f2Mf1(), translation(r) { init(); } @@ -117,8 +111,7 @@ vpFeatureTranslation::vpFeatureTranslation( vpFeature::cdMc or vpFeature::cMo. */ -vpFeatureTranslation::vpFeatureTranslation( - vpHomogeneousMatrix &f2Mf1_, vpFeatureTranslationRepresentationType r) +vpFeatureTranslation::vpFeatureTranslation(vpHomogeneousMatrix &f2Mf1_, vpFeatureTranslationRepresentationType r) : f2Mf1(), translation(r) { init(); @@ -154,8 +147,7 @@ void vpFeatureTranslation::buildFrom(const vpHomogeneousMatrix &f2Mf1_) vpFeatureTranslation::cMo. \sa getFeatureTranslationType() */ -void vpFeatureTranslation::setFeatureTranslationType( - const vpFeatureTranslationRepresentationType r) +void vpFeatureTranslation::setFeatureTranslationType(const vpFeatureTranslationRepresentationType r) { translation = r; } @@ -200,8 +192,7 @@ void vpFeatureTranslation::set_Tz(const double t_z) { s[2] = t_z; } setFeatureTranslationType() */ -vpFeatureTranslation::vpFeatureTranslationRepresentationType -vpFeatureTranslation::getFeatureTranslationType() const +vpFeatureTranslation::vpFeatureTranslationRepresentationType vpFeatureTranslation::getFeatureTranslationType() const { return translation; } @@ -290,8 +281,8 @@ double vpFeatureTranslation::get_Tz() const { return s[2]; } subset visual feature: \code - vpMatrix L_xy = s.interaction( vpFeatureTranslation::selectTx() | - vpFeatureTranslation::selectTy() ); \endcode + 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$ @@ -508,24 +499,21 @@ vpMatrix vpFeatureTranslation::interaction(const unsigned int select) the code becomes: \code // Compute the interaction matrix for the t_y, t_z features - vpMatrix L_yz = s.interaction( vpFeatureTranslation::selectTy() | - vpFeatureTranslation::selectTz() ); + 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 e = s.error(s_star, vpFeatureTranslation::selectTy() | vpFeatureTranslation::selectTz()); + \endcode */ -vpColVector vpFeatureTranslation::error(const vpBasicFeature &s_star, - const unsigned int select) +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 !")); + throw(vpFeatureException(vpFeatureException::badInitializationError, "s* should be zero !")); } } @@ -595,8 +583,10 @@ void vpFeatureTranslation::print(const unsigned int select) const \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 s(vpFeatureTranslation::cMo); + s_star = s.duplicate(); + // s_star is now a vpFeatureTranslation + \endcode */ vpFeatureTranslation *vpFeatureTranslation::duplicate() const @@ -616,10 +606,8 @@ vpFeatureTranslation *vpFeatureTranslation::duplicate() const Not implemented. */ -void vpFeatureTranslation::display(const vpCameraParameters & /* cam */, - const vpImage & /* I */, - const vpColor & /* color */, - unsigned int /* thickness */) const +void vpFeatureTranslation::display(const vpCameraParameters & /* cam */, const vpImage & /* I */, + const vpColor & /* color */, unsigned int /* thickness */) const { static int firsttime = 0; @@ -635,10 +623,8 @@ void vpFeatureTranslation::display(const vpCameraParameters & /* cam */, Not implemented. */ -void vpFeatureTranslation::display(const vpCameraParameters & /* cam */, - const vpImage & /* I */, - const vpColor & /* color */, - unsigned int /* thickness */) const +void vpFeatureTranslation::display(const vpCameraParameters & /* cam */, const vpImage & /* I */, + const vpColor & /* color */, unsigned int /* thickness */) const { static int firsttime = 0; @@ -669,8 +655,8 @@ void vpFeatureTranslation::display(const vpCameraParameters & /* cam */, vpServo task; ... // Add the (tx,ty) subset features from 3D translation to the task - task.addFeature(t, vpFeatureTranslation::selectTx() | - vpFeatureTranslation::selectTy()); \endcode + task.addFeature(t, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy()); + \endcode - With the feature type cMcd: \code @@ -678,8 +664,8 @@ void vpFeatureTranslation::display(const vpCameraParameters & /* cam */, vpServo task; ... // Add the (tx,ty) subset features from 3D translation to the task - task.addFeature(t, vpFeatureTranslation::selectTx() | - vpFeatureTranslation::selectTy()); \endcode + task.addFeature(t, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy()); + \endcode - With the feature type cMo: \code @@ -688,8 +674,8 @@ void vpFeatureTranslation::display(const vpCameraParameters & /* cam */, vpServo task; ... // Add the (tx,ty) subset features from 3D translation to the task - task.addFeature(t, t_star, vpFeatureTranslation::selectTx() | - vpFeatureTranslation::selectTy()); \endcode + task.addFeature(t, t_star, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy()); + \endcode \sa selectTy(), selectTz() @@ -715,8 +701,8 @@ unsigned int vpFeatureTranslation::selectTx() { return FEATURE_LINE[0]; } vpServo task; ... // Add the (tx,ty) subset features from 3D translation to the task - task.addFeature(t, vpFeatureTranslation::selectTx() | - vpFeatureTranslation::selectTy()); \endcode + task.addFeature(t, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy()); + \endcode - With the feature type cMcd: \code @@ -724,8 +710,8 @@ unsigned int vpFeatureTranslation::selectTx() { return FEATURE_LINE[0]; } vpServo task; ... // Add the (tx,ty) subset features from 3D translation to the task - task.addFeature(t, vpFeatureTranslation::selectTx() | - vpFeatureTranslation::selectTy()); \endcode + task.addFeature(t, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy()); + \endcode - With the feature type cMo: \code @@ -734,8 +720,8 @@ unsigned int vpFeatureTranslation::selectTx() { return FEATURE_LINE[0]; } vpServo task; ... // Add the (tx,ty) subset features from 3D translation to the task - task.addFeature(t, t_star, vpFeatureTranslation::selectTx() | - vpFeatureTranslation::selectTy()); \endcode + task.addFeature(t, t_star, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy()); + \endcode \sa selectTx(), selectTz() */ diff --git a/modules/visual_features/src/visual-feature/vpFeatureVanishingPoint.cpp b/modules/visual_features/src/visual-feature/vpFeatureVanishingPoint.cpp index 3f27514bf8..b7623d0b8c 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureVanishingPoint.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureVanishingPoint.cpp @@ -161,8 +161,7 @@ vpMatrix vpFeatureVanishingPoint::interaction(const unsigned int select) /*! 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 vpFeatureVanishingPoint::error(const vpBasicFeature &s_star, const unsigned int select) { vpColVector e(0); @@ -214,10 +213,8 @@ void vpFeatureVanishingPoint::buildFrom(const double _x, const double _y) \param thickness : Thickness of the feature representation. */ -void vpFeatureVanishingPoint::display(const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color, - unsigned int thickness) const +void vpFeatureVanishingPoint::display(const vpCameraParameters &cam, const vpImage &I, + const vpColor &color, unsigned int thickness) const { try { double x, y; @@ -241,9 +238,7 @@ void vpFeatureVanishingPoint::display(const vpCameraParameters &cam, \param thickness : Thickness of the feature representation. */ -void vpFeatureVanishingPoint::display(const vpCameraParameters &cam, - const vpImage &I, - const vpColor &color, +void vpFeatureVanishingPoint::display(const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) const { try { diff --git a/modules/visual_features/src/visual-feature/vpGenericFeature.cpp b/modules/visual_features/src/visual-feature/vpGenericFeature.cpp index ff91a3c071..ab8800b44e 100644 --- a/modules/visual_features/src/visual-feature/vpGenericFeature.cpp +++ b/modules/visual_features/src/visual-feature/vpGenericFeature.cpp @@ -65,8 +65,7 @@ void vpGenericFeature::init() { s = 0; } this constructor. */ -vpGenericFeature::vpGenericFeature() - : L(), err(), errorStatus(errorNotInitalized) +vpGenericFeature::vpGenericFeature() : L(), err(), errorStatus(errorNotInitalized) { /* vpERROR_TRACE("You are not allow to use this constructor ") ; @@ -85,8 +84,7 @@ vpGenericFeature::vpGenericFeature() \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) +vpGenericFeature::vpGenericFeature(unsigned int dimension_gen_s) : L(), err(), errorStatus(errorNotInitalized) { this->dim_s = dimension_gen_s; s.resize(dimension_gen_s); @@ -106,9 +104,8 @@ 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")); + throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between error dimension" + "and feature dimension")); } errorStatus = errorInitialized; err = error_vector; @@ -166,18 +163,16 @@ void vpGenericFeature::setError(const vpColVector &error_vector) 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 + s.error(s_star, vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]); + \endcode */ -vpColVector vpGenericFeature::error(const vpBasicFeature &s_star, - const unsigned int select) +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")); + throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s* dimension " + "and feature dimension")); } vpColVector e(0); @@ -187,9 +182,8 @@ vpColVector vpGenericFeature::error(const vpBasicFeature &s_star, 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")); + 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; @@ -266,9 +260,8 @@ vpColVector vpGenericFeature::error(const unsigned int select) 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")); + 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++) @@ -342,23 +335,20 @@ vpColVector vpGenericFeature::error(const unsigned int select) // 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 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 << "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")); + throw(vpFeatureException(vpFeatureException::notInitializedError, "size mismatch between s* dimension " + "and feature dimension")); } vpMatrix Ls; @@ -395,9 +385,8 @@ void vpGenericFeature::setInteractionMatrix(const vpMatrix &L_) ; vpERROR_TRACE("size mismatch between interaction matrix size " "and feature dimension"); - throw(vpFeatureException(vpFeatureException::sizeMismatchError, - "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_; @@ -419,9 +408,8 @@ 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")); + throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s dimension" + "and feature dimension")); } this->s = s_vector; } @@ -441,9 +429,8 @@ 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")); + throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s dimension" + "and feature dimension")); } s_vector = this->s; } @@ -462,16 +449,14 @@ void vpGenericFeature::get_s(vpColVector &s_vector) const 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) +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")); + throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters" + "and feature dimension")); } s[0] = s0; s[1] = s1; @@ -498,9 +483,8 @@ 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")); + throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters" + "and feature dimension")); } s0 = s[0]; s1 = s[1]; @@ -524,9 +508,8 @@ 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")); + throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters" + "and feature dimension")); } s[0] = s0; s[1] = s1; @@ -549,9 +532,8 @@ 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")); + throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters" + "and feature dimension")); } s0 = s[0]; s1 = s[1]; @@ -572,9 +554,8 @@ 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")); + throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters" + "and feature dimension")); } s[0] = s0; } @@ -594,9 +575,8 @@ 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")); + throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters" + "and feature dimension")); } s0 = s[0]; } @@ -645,10 +625,8 @@ vpGenericFeature *vpGenericFeature::duplicate() const /*! Not implemented. */ -void vpGenericFeature::display(const vpCameraParameters & /* cam */, - const vpImage & /* I */, - const vpColor & /* color */, - unsigned int /* thickness */) const +void vpGenericFeature::display(const vpCameraParameters & /* cam */, const vpImage & /* I */, + const vpColor & /* color */, unsigned int /* thickness */) const { static int firsttime = 0; @@ -662,10 +640,8 @@ void vpGenericFeature::display(const vpCameraParameters & /* cam */, /*! Not implemented. */ -void vpGenericFeature::display(const vpCameraParameters & /* cam */, - const vpImage & /* I */, - const vpColor & /* color */, - unsigned int /* thickness */) const +void vpGenericFeature::display(const vpCameraParameters & /* cam */, const vpImage & /* I */, + const vpColor & /* color */, unsigned int /* thickness */) const { static int firsttime = 0; diff --git a/modules/visual_features/test/feature/testPoint.cpp b/modules/visual_features/test/feature/testPoint.cpp index 4d6dd1323a..a14a990949 100644 --- a/modules/visual_features/test/feature/testPoint.cpp +++ b/modules/visual_features/test/feature/testPoint.cpp @@ -64,8 +64,7 @@ int main() vpTRACE("set point coordinates in the world frame "); point.setWorldCoordinates(0, 0, 0); - std::cout << "------------------------------------------------------" - << std::endl; + std::cout << "------------------------------------------------------" << std::endl; vpTRACE("test the projection "); point.track(cMo); @@ -77,8 +76,7 @@ int main() vpTRACE("2D coordinates "); std::cout << point.get_x() << " " << point.get_y() << std::endl; - std::cout << "------------------------------------------------------" - << std::endl; + std::cout << "------------------------------------------------------" << std::endl; vpTRACE("test the interaction matrix "); vpFeaturePoint p; @@ -105,8 +103,7 @@ int main() L = p.interaction(vpFeaturePoint::selectAll()); std::cout << L << std::endl; - std::cout << "------------------------------------------------------" - << std::endl; + std::cout << "------------------------------------------------------" << std::endl; vpTRACE("test the error "); try { @@ -141,8 +138,7 @@ int main() } catch (vpException &me) { std::cout << me << std::endl; } - std::cout << "------------------------------------------------------" - << std::endl; + std::cout << "------------------------------------------------------" << std::endl; vpTRACE("test the dimension"); unsigned int dim; dim = p.getDimension(); @@ -158,8 +154,7 @@ int main() std::cout << "Dimension = " << dim << std::endl; vpTRACE("\t X & Y"); - dim = - p.getDimension(vpFeaturePoint::selectX() | vpFeaturePoint::selectY()); + dim = p.getDimension(vpFeaturePoint::selectX() | vpFeaturePoint::selectY()); std::cout << "Dimension = " << dim << std::endl; vpTRACE("\t selectAll"); diff --git a/modules/vs/include/visp3/vs/vpAdaptiveGain.h b/modules/vs/include/visp3/vs/vpAdaptiveGain.h index 8cd99370e0..0e2a79c488 100644 --- a/modules/vs/include/visp3/vs/vpAdaptiveGain.h +++ b/modules/vs/include/visp3/vs/vpAdaptiveGain.h @@ -75,13 +75,15 @@ is to reduce the time to convergence in order to speed up the servo. The following example shows how to use this class in order to use an adaptive gain with the following parameters \f$\lambda_0 = 4\f$, -\f$\lambda_\infty = 0.4 \f$ and \f$\lambda'_0 = 30\f$. \code #include - #include +\f$\lambda_\infty = 0.4 \f$ and \f$\lambda'_0 = 30\f$. + +\code +#include +#include int main() { - vpAdaptiveGain lambda(4, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and -lambda'(0)=30 + vpAdaptiveGain lambda(4, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30 vpServo servo; servo.setLambda(lambda); @@ -95,8 +97,11 @@ lambda'(0)=30 This other example shows how to use this class in order to set a constant gain \f$\lambda = 0.5\f$ that will ensure an exponential decrease of the task -error. \code #include #include - +error. + +\code +#include +#include int main() { @@ -135,15 +140,13 @@ class VISP_EXPORT vpAdaptiveGain vpAdaptiveGain(); explicit vpAdaptiveGain(double c); - vpAdaptiveGain(double gain_at_zero, double gain_at_infinity, - double slope_at_zero); + 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); + void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero); /* --- MODIFIORS ---------------------------------------------------------- */ @@ -201,8 +204,7 @@ class VISP_EXPORT vpAdaptiveGain /* --- IOSTREAM ----------------------------------------------------------- */ - friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, - const vpAdaptiveGain &lambda); + friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAdaptiveGain &lambda); }; #endif diff --git a/modules/vs/include/visp3/vs/vpServo.h b/modules/vs/include/visp3/vs/vpServo.h index 5167282671..84be04b310 100644 --- a/modules/vs/include/visp3/vs/vpServo.h +++ b/modules/vs/include/visp3/vs/vpServo.h @@ -103,9 +103,8 @@ int main() 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 + 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 @@ -138,8 +137,7 @@ Default initialization to zero 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 + } 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. @@ -203,8 +201,8 @@ class VISP_EXPORT vpServo } vpServoIteractionMatrixType; typedef enum { - TRANSPOSE, /*!< In the control law (see vpServo::vpServoType), uses the - transpose instead of the pseudo inverse. */ + 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; @@ -217,7 +215,7 @@ class VISP_EXPORT vpServo 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. */ + MINIMUM /*!< Same as vpServo::vpServoPrintType::ERROR_VECTOR. */ } vpServoPrintType; // private: @@ -252,11 +250,9 @@ class VISP_EXPORT vpServo virtual ~vpServo(); // create a new ste of two visual features - void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, - const unsigned int select = vpBasicFeature::FEATURE_ALL); + 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); + void addFeature(vpBasicFeature &s, const unsigned int select = vpBasicFeature::FEATURE_ALL); // compute the desired control law vpColVector computeControlLaw(); @@ -279,11 +275,12 @@ 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 + 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 @@ -351,21 +348,17 @@ corresponding to the visual servoing vpColVector e = task.getError(); // destruction (memory deallocation if required) void kill(); - void print(const vpServo::vpServoPrintType display_level = ALL, - std::ostream &os = std::cout); + 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); + 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; + 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; void setCameraDoF(const vpColVector &dof); @@ -397,10 +390,8 @@ corresponding to the visual servoing vpColVector e = task.getError(); details. \param interactionMatrixInversion : How is the inverse computed. See vpServo::vpServoInversionType for more details. */ - void setInteractionMatrixType( - const vpServoIteractionMatrixType &interactionMatrixType, - const vpServoInversionType &interactionMatrixInversion = - PSEUDO_INVERSE); + void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, + const vpServoInversionType &interactionMatrixInversion = PSEUDO_INVERSE); /*! Set the gain \f$\lambda\f$ used in the control law (see @@ -429,8 +420,7 @@ corresponding to the visual servoing vpColVector e = task.getError(); 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) + 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); } @@ -654,8 +644,8 @@ corresponding to the visual servoing vpColVector e = task.getError(); 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[ + \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] diff --git a/modules/vs/include/visp3/vs/vpServoData.h b/modules/vs/include/visp3/vs/vpServoData.h index e529475a80..db4d956653 100644 --- a/modules/vs/include/visp3/vs/vpServoData.h +++ b/modules/vs/include/visp3/vs/vpServoData.h @@ -74,24 +74,17 @@ class VISP_EXPORT vpServoData public: #ifndef DOXYGEN_SHOULD_SKIP_THIS vpServoData(const vpServoData &sd) - : velocityFile(), errorFile(), errorNormFile(), sFile(), sStarFile(), - vNormFile(), cmDeg(false) + : velocityFile(), errorFile(), errorNormFile(), sFile(), sStarFile(), vNormFile(), cmDeg(false) { *this = sd; } vpServoData &operator=(const vpServoData &) { - throw vpException(vpException::functionNotImplementedError, - "Not implemented!"); + throw vpException(vpException::functionNotImplementedError, "Not implemented!"); } #endif - vpServoData() - : velocityFile(), errorFile(), errorNormFile(), sFile(), sStarFile(), - vNormFile(), cmDeg(false) - { - ; - } + vpServoData() : velocityFile(), errorFile(), errorNormFile(), sFile(), sStarFile(), vNormFile(), cmDeg(false) { ; } virtual ~vpServoData() { ; } //! velocity output in cm and deg diff --git a/modules/vs/include/visp3/vs/vpServoDisplay.h b/modules/vs/include/visp3/vs/vpServoDisplay.h index 417af1b0c3..23974a3732 100644 --- a/modules/vs/include/visp3/vs/vpServoDisplay.h +++ b/modules/vs/include/visp3/vs/vpServoDisplay.h @@ -62,15 +62,11 @@ class VISP_EXPORT vpServoDisplay { public: - static void display(const vpServo &s, const vpCameraParameters &cam, - const vpImage &I, - vpColor currentColor = vpColor::green, - vpColor desiredColor = vpColor::red, + static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage &I, + vpColor currentColor = vpColor::green, vpColor desiredColor = vpColor::red, unsigned int thickness = 1); - static void display(const vpServo &s, const vpCameraParameters &cam, - const vpImage &I, - vpColor currentColor = vpColor::green, - vpColor desiredColor = vpColor::red, + static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage &I, + vpColor currentColor = vpColor::green, vpColor desiredColor = vpColor::red, unsigned int thickness = 1); }; diff --git a/modules/vs/include/visp3/vs/vpServoException.h b/modules/vs/include/visp3/vs/vpServoException.h index 597b6a9311..bfff285034 100644 --- a/modules/vs/include/visp3/vs/vpServoException.h +++ b/modules/vs/include/visp3/vs/vpServoException.h @@ -89,11 +89,7 @@ class VISP_EXPORT vpServoException : public vpException setMessage(format, args); va_end(args); } - vpServoException(const int id, const std::string &msg) - : vpException(id, msg) - { - ; - } + vpServoException(const int id, const std::string &msg) : vpException(id, msg) { ; } explicit vpServoException(const int id) : vpException(id) { ; } }; diff --git a/modules/vs/src/vpAdaptiveGain.cpp b/modules/vs/src/vpAdaptiveGain.cpp index 5d798206d7..aced41cbac 100644 --- a/modules/vs/src/vpAdaptiveGain.cpp +++ b/modules/vs/src/vpAdaptiveGain.cpp @@ -81,11 +81,7 @@ vpAdaptiveGain::vpAdaptiveGain() : coeff_a(), coeff_b(), coeff_c(), lambda(1.) \param c : Value of the constant gain. A typical value is 0.5. */ -vpAdaptiveGain::vpAdaptiveGain(double c) - : coeff_a(), coeff_b(), coeff_c(), lambda(1.) -{ - initFromConstant(c); -} +vpAdaptiveGain::vpAdaptiveGain(double c) : coeff_a(), coeff_b(), coeff_c(), lambda(1.) { initFromConstant(c); } /*! Constructor that initializes the gain as adaptive. @@ -97,8 +93,7 @@ vpAdaptiveGain::vpAdaptiveGain(double c) \f$\lambda'_0\f$. */ -vpAdaptiveGain::vpAdaptiveGain(double gain_at_zero, double gain_at_infinity, - double slope_at_zero) +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); @@ -134,8 +129,7 @@ void vpAdaptiveGain::initFromConstant(const double c) */ void vpAdaptiveGain::initFromVoid(void) { - this->initStandard(vpAdaptiveGain::DEFAULT_LAMBDA_ZERO, - vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY, + this->initStandard(vpAdaptiveGain::DEFAULT_LAMBDA_ZERO, vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY, vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE); return; } @@ -150,9 +144,7 @@ void vpAdaptiveGain::initFromVoid(void) expected slope of \f$\lambda(||{\bf e}||)\f$ when \f$||{\bf e}||=0\f$: \f$\lambda'_0\f$. */ -void vpAdaptiveGain::initStandard(const double gain_at_zero, - const double gain_at_infinity, - const double slope_at_zero) +void vpAdaptiveGain::initStandard(const double gain_at_zero, const double gain_at_infinity, const double slope_at_zero) { this->coeff_a = gain_at_zero - gain_at_infinity; // if (0 == this ->coeff_a) @@ -285,10 +277,7 @@ double vpAdaptiveGain::limitValue(void) const \sa value() */ -double vpAdaptiveGain::operator()(const double x) const -{ - return this->value(x); -} +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 = @@ -311,10 +300,7 @@ double vpAdaptiveGain::operator()(void) const { return this->limitValue(); } \return It returns the value of the computed gain. */ -double vpAdaptiveGain::operator()(const vpColVector &x) const -{ - return this->value(x.infinityNorm()); -} +double vpAdaptiveGain::operator()(const vpColVector &x) const { return this->value(x.infinityNorm()); } /* -------------------------------------------------------------------------- */ @@ -330,11 +316,9 @@ double vpAdaptiveGain::operator()(const vpColVector &x) const \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) +VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAdaptiveGain &lambda) { - os << "Zero= " << lambda.coeff_a + lambda.coeff_c - << "\tInf= " << lambda.coeff_c + os << "Zero= " << lambda.coeff_a + lambda.coeff_c << "\tInf= " << lambda.coeff_c << "\tSlope= " << lambda.coeff_a * lambda.coeff_b; return os; diff --git a/modules/vs/src/vpServo.cpp b/modules/vs/src/vpServo.cpp index afefcba98c..b7432672c8 100644 --- a/modules/vs/src/vpServo.cpp +++ b/modules/vs/src/vpServo.cpp @@ -67,15 +67,12 @@ */ 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(), iscJcIdentity(true), cJc(6, 6) + : 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(), iscJcIdentity(true), cJc(6, 6) { cJc.eye(); } @@ -95,15 +92,12 @@ vpServo::vpServo() */ 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(), iscJcIdentity(true), cJc(6, 6) + : 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(), + iscJcIdentity(true), cJc(6, 6) { cJc.eye(); } @@ -201,17 +195,14 @@ void vpServo::kill() // kill the current and desired feature lists // current list - for (std::list::iterator it = featureList.begin(); - it != featureList.end(); ++it) { + for (std::list::iterator it = featureList.begin(); it != featureList.end(); ++it) { if ((*it)->getDeallocate() == vpBasicFeature::vpServo) { delete (*it); (*it) = NULL; } } // desired list - for (std::list::iterator it = - desiredFeatureList.begin(); - it != desiredFeatureList.end(); ++it) { + for (std::list::iterator it = desiredFeatureList.begin(); it != desiredFeatureList.end(); ++it) { if ((*it)->getDeallocate() == vpBasicFeature::vpServo) { delete (*it); (*it) = NULL; @@ -265,7 +256,9 @@ considered dof: - dof[5] = 1 if rotation along Z is controled, 0 otherwise; The following example shows how to use this function to control only wx, wy -like a pan/tilt: \code #include +like a pan/tilt: +\code +#include #include int main() @@ -317,8 +310,7 @@ void vpServo::setCameraDoF(const vpColVector &dof) \param os : Output stream. */ -void vpServo::print(const vpServo::vpServoPrintType displayLevel, - std::ostream &os) +void vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os) { switch (displayLevel) { case vpServo::ALL: { @@ -356,15 +348,14 @@ void vpServo::print(const vpServo::vpServoPrintType displayLevel, std::list::const_iterator it_s_star; std::list::const_iterator it_select; - for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); - it_s != featureList.end(); ++it_s, ++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(); + 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)); @@ -425,8 +416,8 @@ void vpServo::print(const vpServo::vpServoPrintType displayLevel, std::list::const_iterator it_s; std::list::const_iterator it_select; - for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); - it_s != featureList.end(); ++it_s, ++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)); } @@ -438,8 +429,7 @@ void vpServo::print(const vpServo::vpServoPrintType displayLevel, std::list::const_iterator it_s_star; std::list::const_iterator it_select; - for (it_s_star = desiredFeatureList.begin(), - it_select = featureSelectionList.begin(); + 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)); @@ -486,22 +476,25 @@ void vpServo::print(const vpServo::vpServoPrintType displayLevel, 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; + 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; + 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) +void vpServo::addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select) { featureList.push_back(&s_cur); desiredFeatureList.push_back(&s_star); @@ -518,7 +511,8 @@ void vpServo::addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, 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 + {\bf u} =(\theta u_x, \theta u_y, \theta u_z)\f$ feature: + \code vpFeatureThetaU s(vpFeatureThetaU::cRcd); ... vpServo task; @@ -526,7 +520,9 @@ void vpServo::addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, \endcode For example to use only the \f$\theta u_x\f$ feature, the previous code - becomes: \code vpFeatureThetaU s(vpFeatureThetaU::cRcd); + becomes: + \code + vpFeatureThetaU s(vpFeatureThetaU::cRcd); ... vpServo task; task.addFeature(s, vpFeatureThetaU::selectTUx); @@ -567,30 +563,27 @@ unsigned int vpServo::getDimension() const std::list::const_iterator it_s; std::list::const_iterator it_select; - for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); - it_s != featureList.end(); ++it_s, ++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) +void vpServo::setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrix_type, + const vpServoInversionType &interactionMatrixInversion) { this->interactionMatrixType = interactionMatrix_type; this->inversionType = interactionMatrixInversion; } -static void computeInteractionMatrixFromList( - const std::list &featureList, - const std::list &featureSelectionList, vpMatrix &L) +static void computeInteractionMatrixFromList(const std::list &featureList, + const std::list &featureSelectionList, vpMatrix &L) { if (featureList.empty()) { vpERROR_TRACE("feature list empty, cannot compute Ls"); - throw(vpServoException(vpServoException::noFeatureError, - "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. @@ -624,8 +617,7 @@ static void computeInteractionMatrixFromList( std::list::const_iterator it; std::list::const_iterator it_select; - for (it = featureList.begin(), it_select = featureSelectionList.begin(); - it != featureList.end(); ++it, ++it_select) { + for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select) { /* Get s. */ matrixTmp = (*it)->interaction(*it_select); unsigned int rowMatrixTmp = matrixTmp.getRows(); @@ -666,8 +658,7 @@ vpMatrix vpServo::computeInteractionMatrix() switch (interactionMatrixType) { case CURRENT: { try { - computeInteractionMatrixFromList(this->featureList, - this->featureSelectionList, L); + computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L); dim_task = L.getRows(); interactionMatrixComputed = true; } @@ -678,10 +669,8 @@ vpMatrix vpServo::computeInteractionMatrix() } break; case DESIRED: { try { - if (interactionMatrixComputed == false || - forceInteractionMatrixComputation == true) { - computeInteractionMatrixFromList(this->desiredFeatureList, - this->featureSelectionList, L); + if (interactionMatrixComputed == false || forceInteractionMatrixComputation == true) { + computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, L); dim_task = L.getRows(); interactionMatrixComputed = true; @@ -694,10 +683,8 @@ vpMatrix vpServo::computeInteractionMatrix() case MEAN: { vpMatrix Lstar(L.getRows(), L.getCols()); try { - computeInteractionMatrixFromList(this->featureList, - this->featureSelectionList, L); - computeInteractionMatrixFromList(this->desiredFeatureList, - this->featureSelectionList, Lstar); + computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L); + computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, Lstar); } catch (...) { throw; } @@ -730,13 +717,11 @@ vpColVector vpServo::computeError() { if (featureList.empty()) { vpERROR_TRACE("feature list empty, cannot compute Ls"); - throw(vpServoException(vpServoException::noFeatureError, - "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")); + throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls")); } try { @@ -789,8 +774,7 @@ vpColVector vpServo::computeError() std::list::const_iterator it_s_star; std::list::const_iterator it_select; - for (it_s = featureList.begin(), it_s_star = desiredFeatureList.begin(), - it_select = featureSelectionList.begin(); + 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); @@ -850,8 +834,7 @@ 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")); + throw(vpServoException(vpServoException::servoError, "No control law have been yet defined")); break; case EYEINHAND_CAMERA: return true; @@ -890,8 +873,7 @@ 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")); + throw(vpServoException(vpServoException::servoError, "No control law have been yet defined")); break; case EYEINHAND_CAMERA: return true; @@ -961,10 +943,9 @@ vpColVector vpServo::computeControlLaw() 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")); + throw(vpServoException(vpServoException::servoError, "Cannot compute control law " + "All the matrices are not correctly" + "initialized")); } } if (testUpdated() == false) { @@ -975,8 +956,7 @@ vpColVector vpServo::computeControlLaw() switch (servoType) { case NONE: vpERROR_TRACE("No control law have been yet defined"); - throw(vpServoException(vpServoException::servoError, - "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: @@ -1116,10 +1096,9 @@ vpColVector vpServo::computeControlLaw(double t) 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")); + throw(vpServoException(vpServoException::servoError, "Cannot compute control law " + "All the matrices are not correctly" + "initialized")); } } if (testUpdated() == false) { @@ -1130,8 +1109,7 @@ vpColVector vpServo::computeControlLaw(double t) switch (servoType) { case NONE: vpERROR_TRACE("No control law have been yet defined"); - throw(vpServoException(vpServoException::servoError, - "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: @@ -1209,8 +1187,7 @@ vpColVector vpServo::computeControlLaw(double t) // 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::epsilon()) { + if (iteration == 0 || std::fabs(t) < std::numeric_limits::epsilon()) { e1_initial = e1; } // Security check. If size of e1_initial and e1 differ, that means that @@ -1268,8 +1245,7 @@ vpColVector vpServo::computeControlLaw(double t) {{\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) +vpColVector vpServo::computeControlLaw(double t, const vpColVector &e_dot_init) { static int iteration = 0; @@ -1280,10 +1256,9 @@ vpColVector vpServo::computeControlLaw(double t, 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")); + throw(vpServoException(vpServoException::servoError, "Cannot compute control law " + "All the matrices are not correctly" + "initialized")); } } if (testUpdated() == false) { @@ -1294,8 +1269,7 @@ vpColVector vpServo::computeControlLaw(double t, switch (servoType) { case NONE: vpERROR_TRACE("No control law have been yet defined"); - throw(vpServoException(vpServoException::servoError, - "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: @@ -1373,8 +1347,7 @@ vpColVector vpServo::computeControlLaw(double t, // 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::epsilon()) { + if (iteration == 0 || std::fabs(t) < std::numeric_limits::epsilon()) { e1_initial = e1; } // Security check. If size of e1_initial and e1 differ, that means that @@ -1382,8 +1355,7 @@ vpColVector vpServo::computeControlLaw(double t, if (e1_initial.getRows() != e1.getRows()) e1_initial = e1; - e = -lambda(e1) * e1 + - (e_dot_init + lambda(e1) * e1_initial) * exp(-mu * t); + e = -lambda(e1) * e1 + (e_dot_init + lambda(e1) * e1_initial) * exp(-mu * t); vpMatrix I; @@ -1489,35 +1461,35 @@ void vpServo::computeProjectionOperators() 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; + 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 + 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; + 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 + 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 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")); + 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() @@ -1590,39 +1562,39 @@ vpColVector vpServo::secondaryTask(const vpColVector &de2dt, 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; + 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 + 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; + 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 + 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, +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")); + 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 @@ -1648,20 +1620,28 @@ vpColVector vpServo::secondaryTask(const vpColVector &e2, /*! 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. +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 +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 +\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) @@ -1673,21 +1653,21 @@ 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). +// 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 +// Compute the velocity corresponding to the visual servoing +vpColVector v = task.computeControlLaw(); +// Compute and add the secondary task for the joint limit avoidance +// using the large projection operator +v += task.secondaryTaskJointLimitAvoidance(q, dq, qmin, qmax) +\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 +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(); @@ -1702,9 +1682,8 @@ vpColVector vpServo::secondaryTaskJointLimitAvoidance( 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")); + throw(vpServoException(vpServoException::dimensionError, "Dimension vector q or dq does not correspont to " + "the number of jacobian columns")); } double lambda_l = 0.0; @@ -1752,14 +1731,10 @@ vpColVector vpServo::secondaryTaskJointLimitAvoidance( 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)); + 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)); + 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; } @@ -1777,9 +1752,10 @@ 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() + 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; } @@ -1787,12 +1763,13 @@ vpMatrix vpServo::getI_WpW() const { return I_WpW; } Return the large projection operator. This operator is updated after a call of computeControlLaw(). -\code + \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() + 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; } @@ -1801,12 +1778,14 @@ vpMatrix vpServo::getLargeP() const { return P; } 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; +{\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() + 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; } /*! @@ -1822,10 +1801,8 @@ computeControlLaw(). \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 + 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() */ @@ -1834,12 +1811,12 @@ vpMatrix vpServo::getTaskJacobianPseudoInverse() const { return J1p; } Return the rank of the task jacobian. The rank is updated after a call of computeControlLaw(). -\code + \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 + 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; } @@ -1850,11 +1827,12 @@ 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 + \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() + 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/vpServoDisplay.cpp b/modules/vs/src/vpServoDisplay.cpp index 16c1162326..8218a86640 100644 --- a/modules/vs/src/vpServoDisplay.cpp +++ b/modules/vs/src/vpServoDisplay.cpp @@ -77,16 +77,14 @@ \param thickness : Thickness of the feature representation. */ -void vpServoDisplay::display(const vpServo &s, const vpCameraParameters &cam, - const vpImage &I, - vpColor currentColor, vpColor desiredColor, - unsigned int thickness) +void vpServoDisplay::display(const vpServo &s, const vpCameraParameters &cam, const vpImage &I, + vpColor currentColor, vpColor desiredColor, unsigned int thickness) { std::list::const_iterator it_s; std::list::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) { + 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); @@ -119,15 +117,14 @@ void vpServoDisplay::display(const vpServo &s, const vpCameraParameters &cam, \param thickness : Thickness of the feature representation. */ -void vpServoDisplay::display(const vpServo &s, const vpCameraParameters &cam, - const vpImage &I, vpColor currentColor, - vpColor desiredColor, unsigned int thickness) +void vpServoDisplay::display(const vpServo &s, const vpCameraParameters &cam, const vpImage &I, + vpColor currentColor, vpColor desiredColor, unsigned int thickness) { std::list::const_iterator it_s; std::list::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) { + 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); diff --git a/modules/vs/test/visual-feature/testFeatureMoment.cpp b/modules/vs/test/visual-feature/testFeatureMoment.cpp index a97e2e3f23..68e732aab5 100644 --- a/modules/vs/test/visual-feature/testFeatureMoment.cpp +++ b/modules/vs/test/visual-feature/testFeatureMoment.cpp @@ -48,12 +48,10 @@ #include // initialize scene in the interface -void initScene(const vpHomogeneousMatrix &cMo, - const vpHomogeneousMatrix &cdMo, vpMomentObject &src, +void initScene(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src, vpMomentObject &dst); -vpMatrix execute(const vpHomogeneousMatrix &cMo, - const vpHomogeneousMatrix &cdMo, vpMomentObject &src, +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); @@ -66,8 +64,7 @@ int main() 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 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); } @@ -89,8 +86,7 @@ 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))); + 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); @@ -155,8 +151,7 @@ int test(double x, double y, double z, double alpha) return 0; } -void initScene(const vpHomogeneousMatrix &cMo, - const vpHomogeneousMatrix &cdMo, vpMomentObject &src, +void initScene(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src, vpMomentObject &dst) { std::vector src_pts; @@ -183,8 +178,7 @@ void initScene(const vpHomogeneousMatrix &cMo, dst.fromVector(dst_pts); } -vpMatrix execute(const vpHomogeneousMatrix &cMo, - const vpHomogeneousMatrix &cdMo, vpMomentObject &src, +vpMatrix execute(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src, vpMomentObject &dst) { vpServo::vpServoIteractionMatrixType interaction_type = vpServo::CURRENT; @@ -219,12 +213,10 @@ vpMatrix execute(const vpHomogeneousMatrix &cMo, //////////////////////////////////// // 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]); + 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); @@ -238,17 +230,13 @@ vpMatrix execute(const vpHomogeneousMatrix &cMo, // 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()); + /// 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(), + task.addFeature(featureMoments.getFeatureCInvariant(), featureMomentsDes.getFeatureCInvariant(), (1 << 10) | (1 << 11)); - task.addFeature(featureMoments.getFeatureAlpha(), - featureMomentsDes.getFeatureAlpha()); + task.addFeature(featureMoments.getFeatureAlpha(), featureMomentsDes.getFeatureAlpha()); task.setLambda(0.4); diff --git a/modules/vs/test/visual-feature/testFeatureSegment.cpp b/modules/vs/test/visual-feature/testFeatureSegment.cpp index 1d0f931f2a..2d5315e3a9 100644 --- a/modules/vs/test/visual-feature/testFeatureSegment.cpp +++ b/modules/vs/test/visual-feature/testFeatureSegment.cpp @@ -79,22 +79,17 @@ int main(int argc, const char **argv) // Parse the command line to set the variables vpParseArgv::vpArgvInfo argTable[] = { #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) - {"-d", vpParseArgv::ARGV_CONSTANT_INT, 0, (char *)&opt_no_display, - "Disable display and graphics viewer."}, + {"-d", vpParseArgv::ARGV_CONSTANT_INT, 0, (char *)&opt_no_display, "Disable display and graphics viewer."}, #endif - {"-normalized", vpParseArgv::ARGV_INT, (char *)NULL, - (char *)&opt_normalized, + {"-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} + {"-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_LEFTOVERS | vpParseArgv::ARGV_NO_ABBREV | vpParseArgv::ARGV_NO_DEFAULTS)) { return (false); } @@ -126,12 +121,9 @@ int main(int argc, const char **argv) display->init(I); #endif - vpHomogeneousMatrix - wMo; // Set to indentity. Robot world frame is equal to object frame - vpHomogeneousMatrix cMo(-0.5, 0.5, 2., vpMath::rad(10), vpMath::rad(20), - vpMath::rad(30)); - vpHomogeneousMatrix cdMo(0., 0., 1., vpMath::rad(0), vpMath::rad(0), - vpMath::rad(0)); + vpHomogeneousMatrix wMo; // Set to indentity. Robot world frame is equal to object frame + vpHomogeneousMatrix cMo(-0.5, 0.5, 2., vpMath::rad(10), vpMath::rad(20), vpMath::rad(30)); + 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 @@ -237,7 +229,7 @@ int main(int argc, const char **argv) #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(0, iter, v); // plot velocities applied to the robot graph->plot(1, iter, task.getError()); // plot error vector } #endif @@ -271,8 +263,7 @@ int main(int argc, const char **argv) #else int main() { - std::cout << "Test empty since visp_robot module is not available.\n" - << std::endl; + std::cout << "Test empty since visp_robot module is not available.\n" << std::endl; return 0; } diff --git a/tutorial/bridge/opencv/tutorial-bridge-opencv.cpp b/tutorial/bridge/opencv/tutorial-bridge-opencv.cpp index e9b851be89..dca52f1383 100644 --- a/tutorial/bridge/opencv/tutorial-bridge-opencv.cpp +++ b/tutorial/bridge/opencv/tutorial-bridge-opencv.cpp @@ -22,8 +22,7 @@ int main() //! [Set ViSP camera parameters] //! [Set OpenCV camera parameters] - cv::Mat K = (cv::Mat_(3, 3) << cam.get_px(), 0, cam.get_u0(), 0, - cam.get_py(), cam.get_v0(), 0, 0, 1); + cv::Mat K = (cv::Mat_(3, 3) << cam.get_px(), 0, cam.get_u0(), 0, cam.get_py(), cam.get_v0(), 0, 0, 1); cv::Mat D = (cv::Mat_(4, 1) << cam.get_kud(), 0, 0, 0); //! [Set OpenCV camera parameters] diff --git a/tutorial/computer-vision/tutorial-homography-from-points.cpp b/tutorial/computer-vision/tutorial-homography-from-points.cpp index e292d63ec3..407a7a0a66 100644 --- a/tutorial/computer-vision/tutorial-homography-from-points.cpp +++ b/tutorial/computer-vision/tutorial-homography-from-points.cpp @@ -18,8 +18,7 @@ int main() //! [Simulation] vpHomogeneousMatrix bMo(0.1, 0, 1, 0, vpMath::rad(15), 0); - vpHomogeneousMatrix aMb(0.2, -0.1, 0.1, vpMath::rad(-3), vpMath::rad(20), - vpMath::rad(5)); + vpHomogeneousMatrix aMb(0.2, -0.1, 0.1, vpMath::rad(-3), vpMath::rad(20), vpMath::rad(5)); vpHomogeneousMatrix aMo = aMb * bMo; //! [Simulation] @@ -39,12 +38,10 @@ int main() //! [Compute homography] vpHomography aHb; vpHomography::DLT(xb, yb, xa, ya, aHb, true); - std::cout << "Estimated homography using DLT:\n" - << aHb / aHb[2][2] << std::endl; + std::cout << "Estimated homography using DLT:\n" << aHb / aHb[2][2] << std::endl; vpHomography::HLM(xb, yb, xa, ya, true, aHb); - std::cout << "Estimated homography using HLM:\n" - << aHb / aHb[2][2] << std::endl; + std::cout << "Estimated homography using HLM:\n" << aHb / aHb[2][2] << std::endl; //! [Compute homography] //! [Get transformation] @@ -72,15 +69,13 @@ int main() vpMeterPixelConversion::convertPoint(cam, xb[3], yb[3], iPb); vpMeterPixelConversion::convertPoint(cam, xa[3], ya[3], iPa); - std::cout << "Ground truth: Point 3 in pixels in frame b: " << iPb - << std::endl; - std::cout << "Ground truth: Point 3 in pixels in frame a: " << iPa - << std::endl; + std::cout << "Ground truth: Point 3 in pixels in frame b: " << iPb << std::endl; + std::cout << "Ground truth: Point 3 in pixels in frame a: " << iPa << std::endl; //! [Get pixel coordinates] //! [Project] // Project the position in pixel of point 3 from the homography - std::cout << "Estimation from homography: Point 3 in pixels in frame a: " - << vpHomography::project(cam, aHb, iPb) << std::endl; + std::cout << "Estimation from homography: Point 3 in pixels in frame a: " << vpHomography::project(cam, aHb, iPb) + << std::endl; //! [Project] } diff --git a/tutorial/computer-vision/tutorial-pose-from-points-image.cpp b/tutorial/computer-vision/tutorial-pose-from-points-image.cpp index 8759efa119..95de3d917a 100644 --- a/tutorial/computer-vision/tutorial-pose-from-points-image.cpp +++ b/tutorial/computer-vision/tutorial-pose-from-points-image.cpp @@ -7,8 +7,7 @@ #include #include -void computePose(std::vector &point, const std::vector &dot, - const vpCameraParameters &cam, bool init, +void computePose(std::vector &point, const std::vector &dot, const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo) { vpPose pose; diff --git a/tutorial/computer-vision/tutorial-pose-from-points-tracking.cpp b/tutorial/computer-vision/tutorial-pose-from-points-tracking.cpp index 289540b568..4cf5a6edfa 100644 --- a/tutorial/computer-vision/tutorial-pose-from-points-tracking.cpp +++ b/tutorial/computer-vision/tutorial-pose-from-points-tracking.cpp @@ -11,16 +11,13 @@ #include #include -void computePose(std::vector &point, const std::vector &dot, - const vpCameraParameters &cam, bool init, +void computePose(std::vector &point, const std::vector &dot, const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo); -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_OPENCV) +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) void track(vpImage &I, std::vector &dot, bool init); #endif -void computePose(std::vector &point, const std::vector &dot, - const vpCameraParameters &cam, bool init, +void computePose(std::vector &point, const std::vector &dot, const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo) { vpPose pose; @@ -47,8 +44,7 @@ void computePose(std::vector &point, const std::vector &dot, pose.computePose(vpPose::VIRTUAL_VS, cMo); } -#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_OPENCV) +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) void track(vpImage &I, std::vector &dot, bool init) { if (init) { @@ -69,10 +65,8 @@ void track(vpImage &I, std::vector &dot, bool init) int main() { -#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)) +#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 I; diff --git a/tutorial/computer-vision/tutorial-pose-from-qrcode-image.cpp b/tutorial/computer-vision/tutorial-pose-from-qrcode-image.cpp index bb7ce0857e..25ac2b209c 100644 --- a/tutorial/computer-vision/tutorial-pose-from-qrcode-image.cpp +++ b/tutorial/computer-vision/tutorial-pose-from-qrcode-image.cpp @@ -7,10 +7,8 @@ #include #include -void computePose(std::vector &point, - const std::vector &ip, - const vpCameraParameters &cam, bool init, - vpHomogeneousMatrix &cMo) +void computePose(std::vector &point, const std::vector &ip, const vpCameraParameters &cam, + bool init, vpHomogeneousMatrix &cMo) { vpPose pose; double x = 0, y = 0; @@ -59,9 +57,8 @@ int main() point.push_back(vpPoint(-0.06, -0.06, 0)); // QCcode point 0 3D coordinates in plane Z=0 point.push_back(vpPoint(0.06, -0.06, - 0)); // QCcode point 1 3D coordinates in plane Z=0 - point.push_back( - vpPoint(0.06, 0.06, 0)); // QCcode point 2 3D coordinates in plane Z=0 + 0)); // QCcode point 1 3D coordinates in plane Z=0 + point.push_back(vpPoint(0.06, 0.06, 0)); // QCcode point 2 3D coordinates in plane Z=0 point.push_back(vpPoint(-0.06, 0.06, 0)); // QCcode point 3 3D coordinates in plane Z=0 @@ -78,35 +75,28 @@ int main() std::ostringstream legend; legend << detector.getNbObjects() << " bar code detected"; - vpDisplay::displayText(I, (int)I.getHeight() - 30, 10, legend.str(), - vpColor::red); + vpDisplay::displayText(I, (int)I.getHeight() - 30, 10, legend.str(), vpColor::red); if (status) { // true if at least one QRcode is detected for (size_t i = 0; i < detector.getNbObjects(); i++) { - std::vector p = detector.getPolygon( - i); // get the four corners location in the image + std::vector p = detector.getPolygon(i); // get the four corners location in the image for (size_t j = 0; j < p.size(); j++) { vpDisplay::displayCross(I, p[j], 14, vpColor::red, 3); std::ostringstream number; number << j; - vpDisplay::displayText(I, p[j] + vpImagePoint(15, 5), - number.str(), vpColor::blue); + vpDisplay::displayText(I, p[j] + vpImagePoint(15, 5), number.str(), vpColor::blue); } computePose(point, p, cam, init, cMo); // resulting pose is available in cMo var - std::cout << "Pose translation (meter): " - << cMo.getTranslationVector().t() << std::endl - << "Pose rotation (quaternion): " - << vpQuaternionVector(cMo.getRotationMatrix()).t() - << std::endl; + std::cout << "Pose translation (meter): " << cMo.getTranslationVector().t() << std::endl + << "Pose rotation (quaternion): " << vpQuaternionVector(cMo.getRotationMatrix()).t() << std::endl; vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3); } } - vpDisplay::displayText(I, (int)I.getHeight() - 15, 10, - "A click to quit...", vpColor::red); + vpDisplay::displayText(I, (int)I.getHeight() - 15, 10, "A click to quit...", vpColor::red); vpDisplay::flush(I); if (vpDisplay::getClick(I, false)) diff --git a/tutorial/detection/barcode/tutorial-barcode-detector-live.cpp b/tutorial/detection/barcode/tutorial-barcode-detector-live.cpp index 883915d650..eecc6a5c31 100644 --- a/tutorial/detection/barcode/tutorial-barcode-detector-live.cpp +++ b/tutorial/detection/barcode/tutorial-barcode-detector-live.cpp @@ -12,8 +12,7 @@ int main(int argc, const char **argv) { -#if (VISP_HAVE_OPENCV_VERSION >= 0x020100) && \ - (defined(VISP_HAVE_ZBAR) || defined(VISP_HAVE_DMTX)) +#if (VISP_HAVE_OPENCV_VERSION >= 0x020100) && (defined(VISP_HAVE_ZBAR) || defined(VISP_HAVE_DMTX)) int opt_device = 0; int opt_barcode = 0; // 0=QRCode, 1=DataMatrix @@ -99,21 +98,18 @@ int main(int argc, const char **argv) std::vector p = detector->getPolygon(i); vpRect bbox = detector->getBBox(i); vpDisplay::displayRectangle(I, bbox, vpColor::green); - vpDisplay::displayText( - I, (int)bbox.getTop() - 20, (int)bbox.getLeft(), - "Message: \"" + detector->getMessage(i) + "\"", vpColor::red); + vpDisplay::displayText(I, (int)bbox.getTop() - 20, (int)bbox.getLeft(), + "Message: \"" + detector->getMessage(i) + "\"", vpColor::red); for (size_t j = 0; j < p.size(); j++) { vpDisplay::displayCross(I, p[j], 14, vpColor::red, 3); std::ostringstream number; number << j; - vpDisplay::displayText(I, p[j] + vpImagePoint(10, 0), - number.str(), vpColor::blue); + vpDisplay::displayText(I, p[j] + vpImagePoint(10, 0), number.str(), vpColor::blue); } } } - vpDisplay::displayText(I, (int)I.getHeight() - 25, 10, - "Click to quit...", vpColor::red); + vpDisplay::displayText(I, (int)I.getHeight() - 25, 10, "Click to quit...", vpColor::red); vpDisplay::flush(I); if (vpDisplay::getClick(I, false)) // a click to exit break; diff --git a/tutorial/detection/barcode/tutorial-barcode-detector.cpp b/tutorial/detection/barcode/tutorial-barcode-detector.cpp index 00c9475f79..1d042e92c3 100644 --- a/tutorial/detection/barcode/tutorial-barcode-detector.cpp +++ b/tutorial/detection/barcode/tutorial-barcode-detector.cpp @@ -11,9 +11,8 @@ int main(int argc, const char **argv) { //! [Macro defined] -#if (defined(VISP_HAVE_ZBAR) || defined(VISP_HAVE_DMTX)) && \ - (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || \ - defined(VISP_HAVE_OPENCV)) +#if (defined(VISP_HAVE_ZBAR) || defined(VISP_HAVE_DMTX)) && \ + (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) //! [Macro defined] try { vpImage I; @@ -38,10 +37,7 @@ int main(int argc, const char **argv) if (std::string(argv[i]) == "--code-type") opt_barcode = atoi(argv[i + 1]); else if (std::string(argv[i]) == "--help") { - std::cout - << "Usage: " << argv[0] - << " [--code-type <0 for QRcode | 1 for DataMatrix>] [--help]" - << std::endl; + std::cout << "Usage: " << argv[0] << " [--code-type <0 for QRcode | 1 for DataMatrix>] [--help]" << std::endl; return 0; } } @@ -68,8 +64,7 @@ int main(int argc, const char **argv) //! [Detection] std::ostringstream legend; legend << detector->getNbObjects() << " bar code detected"; - vpDisplay::displayText(I, (int)I.getHeight() - 30, 10, legend.str(), - vpColor::red); + vpDisplay::displayText(I, (int)I.getHeight() - 30, 10, legend.str(), vpColor::red); //! [Parse detected codes] if (status) { @@ -81,21 +76,18 @@ int main(int argc, const char **argv) //! [Get location] vpDisplay::displayRectangle(I, bbox, vpColor::green); //! [Get message] - vpDisplay::displayText( - I, (int)(bbox.getTop() - 10), (int)bbox.getLeft(), - "Message: \"" + detector->getMessage(i) + "\"", vpColor::red); + vpDisplay::displayText(I, (int)(bbox.getTop() - 10), (int)bbox.getLeft(), + "Message: \"" + detector->getMessage(i) + "\"", vpColor::red); //! [Get message] for (size_t j = 0; j < p.size(); j++) { vpDisplay::displayCross(I, p[j], 14, vpColor::red, 3); std::ostringstream number; number << j; - vpDisplay::displayText(I, p[j] + vpImagePoint(15, 5), number.str(), - vpColor::blue); + vpDisplay::displayText(I, p[j] + vpImagePoint(15, 5), number.str(), vpColor::blue); } } - vpDisplay::displayText(I, (int)I.getHeight() - 15, 10, - "A click to quit...", vpColor::red); + vpDisplay::displayText(I, (int)I.getHeight() - 15, 10, "A click to quit...", vpColor::red); vpDisplay::flush(I); vpDisplay::getClick(I); } diff --git a/tutorial/detection/face/tutorial-face-detector-live-threaded.cpp b/tutorial/detection/face/tutorial-face-detector-live-threaded.cpp index 081409fd7b..0338e1734e 100644 --- a/tutorial/detection/face/tutorial-face-detector-live-threaded.cpp +++ b/tutorial/detection/face/tutorial-face-detector-live-threaded.cpp @@ -10,17 +10,12 @@ #include #include -#if (VISP_HAVE_OPENCV_VERSION >= 0x020200) && \ - (defined(VISP_HAVE_PTHREAD) || defined(_WIN32)) +#if (VISP_HAVE_OPENCV_VERSION >= 0x020200) && (defined(VISP_HAVE_PTHREAD) || defined(_WIN32)) #include // Shared vars -typedef enum { - capture_waiting, - capture_started, - capture_stopped -} t_CaptureState; +typedef enum { capture_waiting, capture_started, capture_stopped } t_CaptureState; t_CaptureState s_capture_state = capture_waiting; bool s_face_available = false; #if defined(VISP_HAVE_V4L2) @@ -186,8 +181,7 @@ vpThread::Return detectionFunction(vpThread::Args args) if (face_found_) { vpMutex::vpScopedLock lock(s_mutex_face); s_face_available = true; - s_face_bbox = - face_detector_.getBBox(0); // Get largest face bounding box + s_face_bbox = face_detector_.getBBox(0); // Get largest face bounding box } } else { vpTime::wait(2); // Sleep 2ms @@ -247,11 +241,9 @@ int main(int argc, const char *argv[]) #endif // Start the threads - vpThread thread_capture((vpThread::Fn)captureFunction, - (vpThread::Args)&cap); + vpThread thread_capture((vpThread::Fn)captureFunction, (vpThread::Args)&cap); vpThread thread_display((vpThread::Fn)displayFunction); - vpThread thread_detection((vpThread::Fn)detectionFunction, - (vpThread::Args)&opt_face_cascade_name); + vpThread thread_detection((vpThread::Fn)detectionFunction, (vpThread::Args)&opt_face_cascade_name); // Wait until thread ends up thread_capture.join(); @@ -266,16 +258,11 @@ int main(int argc, const char *argv[]) int main() { #ifndef VISP_HAVE_OPENCV - std::cout << "You should install OpenCV to make this example working..." - << std::endl; -#elif !defined(_WIN32) && \ - (defined(__unix__) || defined(__unix) || \ - (defined(__APPLE__) && defined(__MACH__))) // UNIX - std::cout << "You should enable pthread usage and rebuild ViSP..." - << std::endl; + std::cout << "You should install OpenCV to make this example working..." << std::endl; +#elif !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX + std::cout << "You should enable pthread usage and rebuild ViSP..." << std::endl; #else - std::cout << "Multi-threading seems not supported on this platform" - << std::endl; + std::cout << "Multi-threading seems not supported on this platform" << std::endl; #endif } diff --git a/tutorial/detection/face/tutorial-face-detector-live.cpp b/tutorial/detection/face/tutorial-face-detector-live.cpp index 16d3c5b770..f4c7f84d8f 100644 --- a/tutorial/detection/face/tutorial-face-detector-live.cpp +++ b/tutorial/detection/face/tutorial-face-detector-live.cpp @@ -88,8 +88,7 @@ int main(int argc, const char *argv[]) #else cap >> frame; // get a new frame from camera vpImageConvert::convert(frame, I); - bool face_found = face_detector.detect( - frame); // We pass frame to avoid an internal image conversion + bool face_found = face_detector.detect(frame); // We pass frame to avoid an internal image conversion #endif //! [Acquisition] @@ -102,20 +101,16 @@ int main(int argc, const char *argv[]) for (size_t i = 0; i < face_detector.getNbObjects(); i++) { vpRect bbox = face_detector.getBBox(i); vpDisplay::displayRectangle(I, bbox, vpColor::green, false, 4); - vpDisplay::displayText( - I, (int)bbox.getTop() - 10, (int)bbox.getLeft(), - "Message: \"" + face_detector.getMessage(i) + "\"", - vpColor::red); + vpDisplay::displayText(I, (int)bbox.getTop() - 10, (int)bbox.getLeft(), + "Message: \"" + face_detector.getMessage(i) + "\"", vpColor::red); } } - vpDisplay::displayText(I, (int)I.getHeight() - 25, 10, - "Click to quit...", vpColor::red); + vpDisplay::displayText(I, (int)I.getHeight() - 25, 10, "Click to quit...", vpColor::red); vpDisplay::flush(I); if (vpDisplay::getClick(I, false)) // a click to exit break; - std::cout << "Loop time: " << vpTime::measureTimeMs() - t << " ms" - << std::endl; + std::cout << "Loop time: " << vpTime::measureTimeMs() - t << " ms" << std::endl; } } catch (vpException &e) { std::cout << e.getMessage() << std::endl; diff --git a/tutorial/detection/face/tutorial-face-detector.cpp b/tutorial/detection/face/tutorial-face-detector.cpp index e671487213..556afb3ac8 100644 --- a/tutorial/detection/face/tutorial-face-detector.cpp +++ b/tutorial/detection/face/tutorial-face-detector.cpp @@ -73,15 +73,12 @@ int main(int argc, const char *argv[]) for (size_t i = 0; i < face_detector.getNbObjects(); i++) { vpRect bbox = face_detector.getBBox(i); vpDisplay::displayRectangle(I, bbox, vpColor::green, false, 4); - vpDisplay::displayText( - I, (int)bbox.getTop() - 10, (int)bbox.getLeft(), - "Message: \"" + face_detector.getMessage(i) + "\"", - vpColor::red); + vpDisplay::displayText(I, (int)bbox.getTop() - 10, (int)bbox.getLeft(), + "Message: \"" + face_detector.getMessage(i) + "\"", vpColor::red); } //! [Get face characteristics] } - vpDisplay::displayText(I, (int)I.getHeight() - 25, 10, - "Click to quit...", vpColor::red); + vpDisplay::displayText(I, (int)I.getHeight() - 25, 10, "Click to quit...", vpColor::red); vpDisplay::flush(I); if (vpDisplay::getClick(I, false)) // a click to exit exit_requested = true; diff --git a/tutorial/detection/matching/tutorial-matching-keypoint-SIFT.cpp b/tutorial/detection/matching/tutorial-matching-keypoint-SIFT.cpp index 319267b1be..49ac5fe942 100644 --- a/tutorial/detection/matching/tutorial-matching-keypoint-SIFT.cpp +++ b/tutorial/detection/matching/tutorial-matching-keypoint-SIFT.cpp @@ -9,9 +9,8 @@ int main() { //! [Define] -#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) && \ - (defined(VISP_HAVE_OPENCV_NONFREE) || \ - defined(VISP_HAVE_OPENCV_XFEATURES2D)) +#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) && \ + (defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)) //! [Define] vpImage I; @@ -25,14 +24,12 @@ int main() // Use L2 distance with a matching done using FLANN (Fast Library for // Approximate Nearest Neighbors) const std::string matcherName = "FlannBased"; - vpKeyPoint::vpFilterMatchingType filterType = - vpKeyPoint::ratioDistanceThreshold; + vpKeyPoint::vpFilterMatchingType filterType = vpKeyPoint::ratioDistanceThreshold; vpKeyPoint keypoint(detectorName, extractorName, matcherName, filterType); //! [Construction] //! [Build Reference] - std::cout << "Reference keypoints=" << keypoint.buildReference(I) - << std::endl; + std::cout << "Reference keypoints=" << keypoint.buildReference(I) << std::endl; //! [Build Reference] //! [Create image] @@ -55,8 +52,7 @@ int main() //! [Display] vpDisplay::display(Idisp); - vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), - vpImagePoint(I.getHeight(), I.getWidth()), + vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()), vpColor::white, 2); //! [Display] @@ -72,9 +68,7 @@ int main() keypoint.getMatchedPoints(i, iPref, iPcur); //! [Get matches] //! [Display matches] - vpDisplay::displayLine(Idisp, iPref, - iPcur + vpImagePoint(0, I.getWidth()), - vpColor::green); + vpDisplay::displayLine(Idisp, iPref, iPcur + vpImagePoint(0, I.getWidth()), vpColor::green); //! [Display matches] } //! [Display flush] diff --git a/tutorial/detection/matching/tutorial-matching-keypoint-homography.cpp b/tutorial/detection/matching/tutorial-matching-keypoint-homography.cpp index 2ac7655313..dfa4e5d234 100644 --- a/tutorial/detection/matching/tutorial-matching-keypoint-homography.cpp +++ b/tutorial/detection/matching/tutorial-matching-keypoint-homography.cpp @@ -17,8 +17,7 @@ int main(int argc, const char **argv) if (method == 0) std::cout << "Uses Ransac to estimate the homography" << std::endl; else - std::cout << "Uses a robust scheme to estimate the homography" - << std::endl; + std::cout << "Uses a robust scheme to estimate the homography" << std::endl; //! [Select method] vpImage I; @@ -31,8 +30,7 @@ int main(int argc, const char **argv) const std::string extractorName = "ORB"; // Hamming distance must be used with ORB const std::string matcherName = "BruteForce-Hamming"; - vpKeyPoint::vpFilterMatchingType filterType = - vpKeyPoint::ratioDistanceThreshold; + vpKeyPoint::vpFilterMatchingType filterType = vpKeyPoint::ratioDistanceThreshold; vpKeyPoint keypoint(detectorName, extractorName, matcherName, filterType); keypoint.buildReference(I); @@ -69,8 +67,7 @@ int main(int argc, const char **argv) reader.acquire(I); Idisp.insert(I, vpImagePoint(0, I.getWidth())); vpDisplay::display(Idisp); - vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), - vpImagePoint(I.getHeight(), I.getWidth()), + vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()), vpColor::white, 2); //! [Matching] @@ -89,10 +86,8 @@ int main(int argc, const char **argv) for (unsigned int i = 0; i < nbMatch; i++) { keypoint.getMatchedPoints(i, iPref[i], iPcur[i]); //! [Pixel conversion] - vpPixelMeterConversion::convertPoint(cam, iPref[i], mPref_x[i], - mPref_y[i]); - vpPixelMeterConversion::convertPoint(cam, iPcur[i], 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] } @@ -100,12 +95,10 @@ int main(int argc, const char **argv) try { double residual; if (method == 0) - vpHomography::ransac( - mPref_x, mPref_y, mPcur_x, mPcur_y, curHref, inliers, residual, - (unsigned int)(mPref_x.size() * 0.25), 2.0 / cam.get_px(), true); + vpHomography::ransac(mPref_x, mPref_y, mPcur_x, mPcur_y, curHref, inliers, residual, + (unsigned int)(mPref_x.size() * 0.25), 2.0 / cam.get_px(), true); else - vpHomography::robust(mPref_x, mPref_y, mPcur_x, mPcur_y, curHref, - inliers, residual, 0.4, 4, true); + vpHomography::robust(mPref_x, mPref_y, mPcur_x, mPcur_y, curHref, inliers, residual, 0.4, 4, true); } catch (...) { std::cout << "Cannot compute homography from matches..." << std::endl; } @@ -122,20 +115,16 @@ int main(int argc, const char **argv) //! [Display contour] vpImagePoint offset(0, I.getWidth()); for (int i = 0; i < 4; i++) { - vpDisplay::displayLine(Idisp, corner_cur[i] + offset, - corner_cur[(i + 1) % 4] + offset, vpColor::blue, - 3); + vpDisplay::displayLine(Idisp, corner_cur[i] + offset, corner_cur[(i + 1) % 4] + offset, vpColor::blue, 3); } //! [Display contour] //! [Display matches] for (unsigned int i = 0; i < nbMatch; i++) { if (inliers[i] == true) - vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, - vpColor::green); + vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, vpColor::green); else - vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, - vpColor::red); + vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, vpColor::red); } //! [Display matches] diff --git a/tutorial/detection/matching/tutorial-matching-keypoint.cpp b/tutorial/detection/matching/tutorial-matching-keypoint.cpp index c8ac01d384..34d565bd4d 100644 --- a/tutorial/detection/matching/tutorial-matching-keypoint.cpp +++ b/tutorial/detection/matching/tutorial-matching-keypoint.cpp @@ -22,11 +22,9 @@ int main() const std::string extractorName = "ORB"; // Hamming distance must be used with ORB const std::string matcherName = "BruteForce-Hamming"; - vpKeyPoint::vpFilterMatchingType filterType = - vpKeyPoint::ratioDistanceThreshold; + vpKeyPoint::vpFilterMatchingType filterType = vpKeyPoint::ratioDistanceThreshold; vpKeyPoint keypoint(detectorName, extractorName, matcherName, filterType); - std::cout << "Reference keypoints=" << keypoint.buildReference(I) - << std::endl; + std::cout << "Reference keypoints=" << keypoint.buildReference(I) << std::endl; //! [Construction] //! [Create image] @@ -49,8 +47,7 @@ int main() //! [Display] vpDisplay::display(Idisp); - vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), - vpImagePoint(I.getHeight(), I.getWidth()), + vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()), vpColor::white, 2); //! [Display] @@ -66,9 +63,7 @@ int main() keypoint.getMatchedPoints(i, iPref, iPcur); //! [Get matches] //! [Display matches] - vpDisplay::displayLine(Idisp, iPref, - iPcur + vpImagePoint(0, I.getWidth()), - vpColor::green); + vpDisplay::displayLine(Idisp, iPref, iPcur + vpImagePoint(0, I.getWidth()), vpColor::green); //! [Display matches] } //! [Display flush] diff --git a/tutorial/detection/matching/tutorial-matching-surf-deprecated.cpp b/tutorial/detection/matching/tutorial-matching-surf-deprecated.cpp index 313cf4c013..0a4c038d36 100644 --- a/tutorial/detection/matching/tutorial-matching-surf-deprecated.cpp +++ b/tutorial/detection/matching/tutorial-matching-surf-deprecated.cpp @@ -41,8 +41,7 @@ int main() //! [Display] vpDisplay::display(Idisp); - vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), - vpImagePoint(I.getHeight(), I.getWidth()), + vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()), vpColor::white, 2); //! [Display] @@ -56,9 +55,7 @@ int main() surf.getMatchedPoints(i, iPref, iPcur); //! [Get matches] //! [Display matches] - vpDisplay::displayLine(Idisp, iPref, - iPcur + vpImagePoint(0, I.getWidth()), - vpColor::green); + vpDisplay::displayLine(Idisp, iPref, iPcur + vpImagePoint(0, I.getWidth()), vpColor::green); //! [Display matches] } //! [Display flush] diff --git a/tutorial/detection/matching/tutorial-matching-surf-homography-deprecated.cpp b/tutorial/detection/matching/tutorial-matching-surf-homography-deprecated.cpp index 42974421ac..6e00c4e582 100644 --- a/tutorial/detection/matching/tutorial-matching-surf-homography-deprecated.cpp +++ b/tutorial/detection/matching/tutorial-matching-surf-homography-deprecated.cpp @@ -17,8 +17,7 @@ int main(int argc, const char **argv) if (method == 0) std::cout << "Uses Ransac to estimate the homography" << std::endl; else - std::cout << "Uses a robust scheme to estimate the homography" - << std::endl; + std::cout << "Uses a robust scheme to estimate the homography" << std::endl; //! [Select method] vpImage I; @@ -62,8 +61,7 @@ int main(int argc, const char **argv) reader.acquire(I); Idisp.insert(I, vpImagePoint(0, I.getWidth())); vpDisplay::display(Idisp); - vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), - vpImagePoint(I.getHeight(), I.getWidth()), + vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()), vpColor::white, 2); //! [Matching] @@ -82,10 +80,8 @@ int main(int argc, const char **argv) vpImagePoint matched_ref, matched_cur; surf.getMatchedPoints(i, matched_ref, matched_cur); //! [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, matched_ref, mPref_x[i], mPref_y[i]); + vpPixelMeterConversion::convertPoint(cam, matched_cur, mPcur_x[i], mPcur_y[i]); //! [Pixel conversion] // Store the image coordinates in pixel of the matched points @@ -96,12 +92,10 @@ int main(int argc, const char **argv) //! [Homography estimation] double residual; if (method == 0) - vpHomography::ransac( - mPref_x, mPref_y, mPcur_x, mPcur_y, curHref, inliers, residual, - (unsigned int)mPref_x.size() / 2, 2.0 / cam.get_px(), true); + vpHomography::ransac(mPref_x, mPref_y, mPcur_x, mPcur_y, curHref, inliers, residual, + (unsigned int)mPref_x.size() / 2, 2.0 / cam.get_px(), true); else - vpHomography::robust(mPref_x, mPref_y, mPcur_x, mPcur_y, curHref, - inliers, residual, 0.4, 4, true); + vpHomography::robust(mPref_x, mPref_y, mPcur_x, mPcur_y, curHref, inliers, residual, 0.4, 4, true); //! [Homography estimation] //! [Projection] @@ -114,20 +108,16 @@ int main(int argc, const char **argv) //! [Display contour] vpImagePoint offset(0, I.getWidth()); for (int i = 0; i < 4; i++) { - vpDisplay::displayLine(Idisp, corner_cur[i] + offset, - corner_cur[(i + 1) % 4] + offset, vpColor::blue, - 3); + vpDisplay::displayLine(Idisp, corner_cur[i] + offset, corner_cur[(i + 1) % 4] + offset, vpColor::blue, 3); } //! [Display contour] //! [Display matches] for (unsigned int i = 0; i < nbMatch; i++) { if (inliers[i] == true) - vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, - vpColor::green); + vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, vpColor::green); else - vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, - vpColor::red); + vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, vpColor::red); } //! [Display matches] diff --git a/tutorial/detection/object/tutorial-detection-object-mbt.cpp b/tutorial/detection/object/tutorial-detection-object-mbt.cpp index 29c533ac4b..0de5eb0e22 100644 --- a/tutorial/detection/object/tutorial-detection-object-mbt.cpp +++ b/tutorial/detection/object/tutorial-detection-object-mbt.cpp @@ -19,9 +19,7 @@ int main(int argc, char **argv) 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